Revize f06d5ae1

Re #8736 - Implement Additional Messages

deltarobot/curvedataserver.cpp

@@ -3 +3 @@
 #include <QDebug>
 #include <QHostAddress>
 #include <QAbstractSocket>
+#include <QVector4D>
 #include <chrono>
 
 
@@ -252 +253 @@
     }
 }
 
+void CurveDataServer::sendActualDirection(float x, float y, float z)
+{
+    QByteArray message = message128(0x4007,
+                                    *((uint32_t*) &x),
+                                    *((uint32_t*) &y),
+                                    *((uint32_t*) &z));
+
+    {
+        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+        for (auto& client : _sockets) {
+                client.socket->write(message);
+        }
+    }
+}
+
+void CurveDataServer::sendTargetDirection(float x, float y, float z)
+{
+    QByteArray message = message128(0x4008,
+                                    *((uint32_t*) &x),
+                                    *((uint32_t*) &y),
+                                    *((uint32_t*) &z));
+
+    {
+        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+        for (auto& client : _sockets) {
+                client.socket->write(message);
+        }
+    }
+}
+
+void CurveDataServer::sendNewCurve(QList<QVector3D> points)
+{
+    int size = points.size();
+
+    QByteArray message = messageVarLength(0xF009, (uint32_t) size * 12);
+
+    for (int i = 0; i < size; i++)
+    {
+        float xf = points[i].x();
+        float yf = points[i].y();
+        float zf = points[i].z();
+
+        uint8_t *x = ((uint8_t*) &xf);
+        uint8_t *y = ((uint8_t*) &yf);
+        uint8_t *z = ((uint8_t*) &zf);
+
+        message[2 + 4 + i*12 + 0] = x[0];
+        message[2 + 4 + i*12 + 1] = x[1];
+        message[2 + 4 + i*12 + 2] = x[2];
+        message[2 + 4 + i*12 + 3] = x[3];
+
+        message[2 + 4 + i*12 + 4] = y[0];
+        message[2 + 4 + i*12 + 5] = y[1];
+        message[2 + 4 + i*12 + 6] = y[2];
+        message[2 + 4 + i*12 + 7] = y[3];
+
+        message[2 + 4 + i*12 + 8] = z[0];
+        message[2 + 4 + i*12 + 9] = z[1];
+        message[2 + 4 + i*12 + 10] = z[2];
+        message[2 + 4 + i*12 + 11] = z[3];
+    }
+
+    {
+        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+        for (auto& client : _sockets) {
+                client.socket->write(message);
+        }
+    }
+}
+
+void CurveDataServer::sendNewCurve(QList<QVector4D> points)
+{
+    QList<QVector3D> curve3d;
+    int itemCount = points.size();
+
+    curve3d.clear();
+    if (itemCount > MAX_CURVE_SIZE)
+        itemCount = MAX_CURVE_SIZE;
+    for (int i = 0; i < itemCount; i++)
+    {
+        curve3d.append(points[i].toVector3D());
+    }
+
+    sendNewCurve(curve3d);
+}
+
 QList<CurveDataServer::Client>::iterator CurveDataServer::clientOf(QTcpSocket *socket) {
     std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
     for (auto it = _sockets.begin(); it != _sockets.end(); it++) {