Revize e6053a29

Re #8740 - Server refactor

deltarobot/curvedataserver.cpp

@@ -4 +4 @@
 #include <QHostAddress>
 #include <QAbstractSocket>
 
+static QByteArray message8(uint16_t messageId, uint8_t content = 0) {
+    QByteArray result(2 + 1, 0);
+    result[0] = ((uint8_t*) &messageId)[0];
+    result[1] = ((uint8_t*) &messageId)[1];
+    result[2] = content;
+    return result;
+}
+
+static QByteArray message16(uint16_t messageId, uint16_t content = 0) {
+    QByteArray result(2 + 2, 0);
+    result[0] = ((uint8_t*) &messageId)[0];
+    result[1] = ((uint8_t*) &messageId)[1];
+    result[2] = ((uint8_t*) &content)[0];
+    result[3] = ((uint8_t*) &content)[1];
+    return result;
+}
+
+static QByteArray message32(uint16_t messageId, uint32_t content = 0) {
+    QByteArray result(2 + 4, 0);
+    result[0] = ((uint8_t*) &messageId)[0];
+    result[1] = ((uint8_t*) &messageId)[1];
+    result[2] = ((uint8_t*) &content)[0];
+    result[3] = ((uint8_t*) &content)[1];
+    result[4] = ((uint8_t*) &content)[2];
+    result[5] = ((uint8_t*) &content)[3];
+    return result;
+}
+
+static QByteArray message64(uint16_t messageId, uint64_t content = 0) {
+    QByteArray result(2 + 8, 0);
+    result[0] = ((uint8_t*) &messageId)[0];
+    result[1] = ((uint8_t*) &messageId)[1];
+    result[2] = ((uint8_t*) &content)[0];
+    result[3] = ((uint8_t*) &content)[1];
+    result[4] = ((uint8_t*) &content)[2];
+    result[5] = ((uint8_t*) &content)[3];
+    result[6] = ((uint8_t*) &content)[4];
+    result[7] = ((uint8_t*) &content)[5];
+    result[8] = ((uint8_t*) &content)[6];
+    result[9] = ((uint8_t*) &content)[7];
+    return result;
+}
+
+static QByteArray message128(uint16_t messageId,
+                             uint32_t contentX = 0,
+                             uint32_t contentY = 0,
+                             uint32_t contentZ = 0,
+                             uint32_t contentW = 0)
+{
+    QByteArray result(2 + 16, 0);
+    result[0] = ((uint8_t*) &messageId)[0];
+    result[1] = ((uint8_t*) &messageId)[1];
+
+    result[2] = ((uint8_t*) &contentX)[0];
+    result[3] = ((uint8_t*) &contentX)[1];
+    result[4] = ((uint8_t*) &contentX)[2];
+    result[5] = ((uint8_t*) &contentX)[3];
+
+    result[6] = ((uint8_t*) &contentY)[0];
+    result[7] = ((uint8_t*) &contentY)[1];
+    result[8] = ((uint8_t*) &contentY)[2];
+    result[9] = ((uint8_t*) &contentY)[3];
+
+    result[10] = ((uint8_t*) &contentZ)[0];
+    result[11] = ((uint8_t*) &contentZ)[1];
+    result[12] = ((uint8_t*) &contentZ)[2];
+    result[13] = ((uint8_t*) &contentZ)[3];
+
+    result[14] = ((uint8_t*) &contentW)[0];
+    result[15] = ((uint8_t*) &contentW)[1];
+    result[16] = ((uint8_t*) &contentW)[2];
+    result[17] = ((uint8_t*) &contentW)[3];
+    return result;
+}
+
+static QByteArray messageVarLength(uint16_t messageId, uint32_t length) {
+    QByteArray result(2 + 4 + length, 0);
+    result[0] = ((uint8_t*) &messageId)[0];
+    result[1] = ((uint8_t*) &messageId)[1];
+    result[2] = ((uint8_t*) &length)[0];
+    result[3] = ((uint8_t*) &length)[1];
+    result[4] = ((uint8_t*) &length)[2];
+    result[5] = ((uint8_t*) &length)[3];
+    return result;
+}
+
 CurveDataServer::CurveDataServer() :
     _server(this),
     _sockets()
@@ -12 +98 @@
     connect(&_server, SIGNAL(newConnection()), this, SLOT(onNewConnection()));
 }
 
-CurveDataServer::~CurveDataServer()
-{
-    // asi vlastne neni co
-}
-
 void CurveDataServer::onNewConnection()
 {
     QTcpSocket *clientSocket = _server.nextPendingConnection();
@@ -48 +129 @@
     }
 }
 
-QByteArray *CurveDataServer::createMessageId(int sizeId, int typeId)
-{
-    QByteArray *id = new QByteArray();
-
-    id->append((sizeId & 0x0F) << 4 + ((typeId & 0x0F00) >> 12));
-    id->append(typeId & 0xFF);
-
-    return id;
-}
-/*
-QByteArray *CurveDataServer::appendFloatToMessage(QByteArray *message, float f)
-{
-    if (!message)
-        return message;
-
-    unsigned char *c = reinterpret_cast<unsigned char *>(&f);
-
-    for (int i = 0; i < 4; i++)
-    {
-        message->append(c[3 - i]);
-    }
-
-    return message;
-}
-*/
 QByteArray *CurveDataServer::putFloatIntoMessage(QByteArray *message, int index, float f)
 {
     if (!message || message->size() < index + 4)
@@ -89 +145 @@
 }
 
 
-void CurveDataServer::sendApplicatorPoint(float x, float y, float z)
+void CurveDataServer::sendActuatorPosition(float x, float y, float z)
 {
-    QByteArray message(18, 0);
-    message[0] = 0x03;
-    message[1] = 0x40;
-
-    putFloatIntoMessage(&message, 2, x);
-    putFloatIntoMessage(&message, 6, y);
-    putFloatIntoMessage(&message, 10, z);
-    putFloatIntoMessage(&message, 14, 0.0);
+    QByteArray message = message128(0x4003,
+                                    *((uint32_t*) &x),
+                                    *((uint32_t*) &y),
+                                    *((uint32_t*) &z));
 
     for (QTcpSocket* socket : _sockets) {
             socket->write(message);
@@ -117 +169 @@
 void CurveDataServer::sendProtocolMagic(QTcpSocket *socket)
 {
     char *messageContent = "DeltaRVr";
-    QByteArray message(10, 0);
-    message[0] = 0x01;
-    message[1] = 0x30;
-
-    for (int i = 0; i < 8; i++)
-    {
-        message[i + 2] = messageContent[i];
-    }
+    QByteArray message = message64(0x3001, *((uint64_t*) messageContent));
 
     socket->write(message);
 
@@ -134 +179 @@
 
 void CurveDataServer::sendVersion(QTcpSocket *socket)
 {
-    QByteArray message(6, 0);
-    message[0] = 0x02;
-    message[1] = 0x20;
-
-    message[2] = 0x01;
+    QByteArray message = message32(0x2002, 1);
 
     socket->write(message);
 

deltarobot/curvedataserver.h

@@ -11 +11 @@
 
 public:
     CurveDataServer();
-    ~CurveDataServer();
-    void sendApplicatorPoint(float x, float y, float z);
+    void sendActuatorPosition(float x, float y, float z);
 
 public slots:
     void onNewConnection();
@@ -20 +19 @@
     void onReadyRead();
 
 private:
-    QByteArray *createMessageId(int sizeId, int typId);
-    //QByteArray *appendFloatToMessage(QByteArray *message, float f);
     QByteArray *putFloatIntoMessage(QByteArray *message, int index, float f);
     void sendPreamble(QTcpSocket *socket);
     void sendProtocolMagic(QTcpSocket *socket);

deltarobot/mainwindow.cpp

@@ -1071 +1071 @@
         connectionGuard->restart();
     }
 #endif
-    cdServer->sendApplicatorPoint(spherePos.x(), spherePos.y(), spherePos.z());
+    cdServer->sendActuatorPosition(spherePos.x(), spherePos.y(), spherePos.z());
 }
 
 //! Příjem dat ze stroje