Commit 36abc43f authored by Beat Küng's avatar Beat Küng

gps: use GPS_RTCM_DATA mavlink message for RTCM, use fragmentation if needed

parent 3a71177d
...@@ -11,7 +11,6 @@ ...@@ -11,7 +11,6 @@
#include "RTCMMavlink.h" #include "RTCMMavlink.h"
#include "MultiVehicleManager.h" #include "MultiVehicleManager.h"
#include "MAVLinkProtocol.h"
#include "Vehicle.h" #include "Vehicle.h"
#include <cstdio> #include <cstdio>
...@@ -24,8 +23,6 @@ RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox) ...@@ -24,8 +23,6 @@ RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox)
void RTCMMavlink::RTCMDataUpdate(QByteArray message) void RTCMMavlink::RTCMDataUpdate(QByteArray message)
{ {
Q_ASSERT(message.size() <= 110); //mavlink message uses a fixed-size buffer
/* statistics */ /* statistics */
_bandwidthByteCounter += message.size(); _bandwidthByteCounter += message.size();
qint64 elapsed = _bandwidthTimer.elapsed(); qint64 elapsed = _bandwidthTimer.elapsed();
...@@ -34,19 +31,38 @@ void RTCMMavlink::RTCMDataUpdate(QByteArray message) ...@@ -34,19 +31,38 @@ void RTCMMavlink::RTCMDataUpdate(QByteArray message)
_bandwidthTimer.restart(); _bandwidthTimer.restart();
_bandwidthByteCounter = 0; _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(); const int maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN;
memcpy(&mavlinkRtcmData.data, message.data(), message.size()); mavlink_gps_rtcm_data_t mavlinkRtcmData;
memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_rtcm_data_t));
if (message.size() < maxMessageLength) {
mavlinkRtcmData.len = message.size();
memcpy(&mavlinkRtcmData.data, message.data(), message.size());
sendMessageToVehicle(mavlinkRtcmData);
} else {
//we need to fragment
int start = 0;
while (start < message.size()) {
int length = std::min(message.size() - start, maxMessageLength);
mavlinkRtcmData.flags = 1; //fragmented
mavlinkRtcmData.len = length;
memcpy(&mavlinkRtcmData.data, message.data() + start, length);
sendMessageToVehicle(mavlinkRtcmData);
start += length;
}
}
}
void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg)
{
QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles();
MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol();
for (int i = 0; i < vehicles.count(); i++) { for (int i = 0; i < vehicles.count(); i++) {
Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]); Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
mavlink_message_t message; mavlink_message_t message;
mavlink_msg_gps_inject_data_encode(mavlinkProtocol->getSystemId(), mavlink_msg_gps_rtcm_data_encode(mavlinkProtocol->getSystemId(),
mavlinkProtocol->getComponentId(), &message, &mavlinkRtcmData); mavlinkProtocol->getComponentId(), &message, &msg);
vehicle->sendMessage(message); vehicle->sendMessage(message);
} }
} }
...@@ -14,6 +14,7 @@ ...@@ -14,6 +14,7 @@
#include <QElapsedTimer> #include <QElapsedTimer>
#include "QGCToolbox.h" #include "QGCToolbox.h"
#include "MAVLinkProtocol.h"
/** /**
** class RTCMMavlink ** class RTCMMavlink
...@@ -30,6 +31,8 @@ public slots: ...@@ -30,6 +31,8 @@ public slots:
void RTCMDataUpdate(QByteArray message); void RTCMDataUpdate(QByteArray message);
private: private:
void sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg);
QGCToolbox& _toolbox; QGCToolbox& _toolbox;
QElapsedTimer _bandwidthTimer; QElapsedTimer _bandwidthTimer;
int _bandwidthByteCounter = 0; int _bandwidthByteCounter = 0;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment