sensor: fix ping360helper using the answering ip instead of ip in the…

… answer
bluerobotics · Jun 10, 2023 · c6cd156 · c6cd156
1 parent cc70609
commit c6cd156
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 2 deletions.
diff --git a/src/sensor/ping360asciiprotocol.cpp b/src/sensor/ping360asciiprotocol.cpp
@@ -7,17 +7,36 @@ QByteArray Ping360AsciiProtocol::discoveryMessage() { return {"Discovery"}; }
 Ping360DiscoveryResponse Ping360AsciiProtocol::decodeDiscoveryResponse(const QString& response)
 {
     auto lines = response.split(QStringLiteral("\r\n"));
+    QString ipAddress;
+
     for (auto& line : lines) {
         line = line.trimmed();
+        if (line.startsWith("IP Address:- ")) {
+            ipAddress = line.remove("IP Address:- ");
+            line = "IP Address:- " + Ping360AsciiProtocol::removeLeadingZerosFromIP(ipAddress);
+        }
     };
 
     if (lines.size() == Ping360DiscoveryResponse::numberOfLines) {
-        return {lines[0], lines[1], lines[2], lines[3]};
+        QStringList splitted = lines[3].split(":- ");
+        if (splitted.size() < 2) {
+            qWarning() << "Invalid format in line[3], ':- ' not found.";
+            return {};
+        }
+        return {lines[0], lines[1], lines[2], splitted[1]};
     }
 
     return {};
 }
 
+QString Ping360AsciiProtocol::removeLeadingZerosFromIP(const QString& line) {
+    QStringList ipParts = line.split(".");
+    for (int i = 0; i < ipParts.size(); i++) {
+        ipParts[i] = QString::number(ipParts[i].toInt());
+    }
+    return ipParts.join(".");
+}
+
 QByteArray Ping360AsciiProtocol::staticIpAddressMessage(const QString& address)
 {
     union Ipv4Union {

diff --git a/src/sensor/ping360asciiprotocol.h b/src/sensor/ping360asciiprotocol.h
@@ -46,6 +46,8 @@ class Ping360AsciiProtocol : public QObject {
      */
     static Ping360DiscoveryResponse decodeDiscoveryResponse(const QString& response);
 
+    static QString removeLeadingZerosFromIP(const QString& line);
+
     /**
      * @brief Get UDP port for this communication protocol
      *

diff --git a/src/sensor/ping360helperservice.cpp b/src/sensor/ping360helperservice.cpp
@@ -77,7 +77,7 @@ void Ping360HelperService::processBroadcastResponses()
         const Ping360DiscoveryResponse decoded = Ping360AsciiProtocol::decodeDiscoveryResponse(datagram.data());
         if (decoded.deviceName.contains("PING360")) {
             emit availableLinkFound(
-                {{LinkType::Udp, {sender.toString(), "12345"}, "Ping360 Port", PingDeviceType::PING360}},
+                {{LinkType::Udp, {decoded.ipAddress, "12345"}, "Ping360 Port", PingDeviceType::PING360}},
                 QStringLiteral("Ping360 Ethernet Protocol Detector"));
         } else {
             qCWarning(PING360HELPERSERVICE) << "Invalid message:" << datagram.data();