Calibration stand via UDP link

Author: aproof

DevDriver.cpp

@@ -134,22 +134,6 @@ int DevDriver::getDevDefAddress() {
     return idUART->devDefAddress();
 }
 
-int DevDriver::dopplerVeloX() {
-    return idDVL->velX();
-}
-
-int DevDriver::dopplerVeloY() {
-    return idDVL->velY();
-}
-
-int DevDriver::dopplerVeloZ() {
-    return idDVL->velZ();
-}
-
-int DevDriver::dopplerDist() {
-    return idDVL->dist();
-}
-
 void DevDriver::dvlChangeMode(bool ismode1, bool ismode2, bool ismode3, bool ismode4, float range_mode4) {
     idDVLMode->setModes(ismode1, ismode2, ismode3, ismode4, range_mode4);
 }
@@ -345,7 +329,7 @@ void DevDriver::setUartState(bool state) {
     }
 }
 
-void DevDriver::askBeaconPosition(IDBinUsblSolution::AskBeacon ask) {
+void DevDriver::askBeaconPosition(IDBinUsblSolution::USBLRequestBeacon ask) {
     if(!m_state.connect) return;
     idUSBL->askBeacon(ask);
 }
@@ -1047,12 +1031,18 @@ void DevDriver::receivedDVLMode(Type type, Version ver, Resp resp) {
 
 void DevDriver::receivedUSBL(Type type, Version ver, Resp resp) {
     Q_UNUSED(type);
-    Q_UNUSED(ver);
-    Q_UNUSED(resp);
 
-    usblSolutionComplete(idUSBL->usblSolution());
-    // emit distComplete(idUSBL->usblSolution().distance_m*1000);
-    // emit attitudeComplete(idUSBL->usblSolution().azimuth_deg, idUSBL->usblSolution().elevation_deg, 0);
+
+
+    if(resp == respNone) {
+        if(ver == Parsers::v0) {
+            emit usblSolutionComplete(idUSBL->usblSolution());
+        } else if(ver == Parsers::v1) {
+            qDebug("usbl p.ver %d", ver);
+            emit beaconActivationComplete(0);
+        }
+    }
+
 }
 
 void DevDriver::process() {

DevDriver.h

@@ -103,14 +103,6 @@ class DevDriver : public QObject
     void setDevDefAddress(int addr);
     int getDevDefAddress();
 
-    int dopplerVeloX();
-    int dopplerVeloY();
-    int dopplerVeloZ();
-    int dopplerDist();
-
-
-
-
     QString devName() { return m_devName; }
     uint32_t devSerialNumber();
     QString devPN();
@@ -174,7 +166,10 @@ class DevDriver : public QObject
     void iqComplete(QByteArray data, uint8_t type);
     void attitudeComplete(float yaw, float pitch, float roll);
     void distComplete(int dist);
+
     void usblSolutionComplete(IDBinUsblSolution::UsblSolution data);
+    void beaconActivationComplete(uint8_t id);
+
     void positionComplete(uint32_t date, uint32_t time, double lat, double lon);
     void chartSetupChanged();
     void dspSetupChanged();
@@ -226,10 +221,10 @@ public slots:
     void setUartState(bool state);
 
     void askBeaconPosition() {
-        IDBinUsblSolution::AskBeacon ask;
+        IDBinUsblSolution::USBLRequestBeacon ask;
         askBeaconPosition(ask);
     }
-    void askBeaconPosition(IDBinUsblSolution::AskBeacon ask);
+    void askBeaconPosition(IDBinUsblSolution::USBLRequestBeacon ask);
     void enableBeaconOnce(float timeout);
 
 protected:

DevQProperty.h

@@ -51,11 +51,6 @@ class DevQProperty : public DevDriver
 
     Q_PROPERTY(int upgradeFWStatus READ upgradeFWStatus NOTIFY upgradeChanged)
 
-    Q_PROPERTY(int dopplerVeloX READ dopplerVeloX NOTIFY dopplerVeloComplete)
-    Q_PROPERTY(int dopplerVeloY READ dopplerVeloY NOTIFY dopplerVeloComplete)
-    Q_PROPERTY(int dopplerVeloZ READ dopplerVeloZ NOTIFY dopplerVeloComplete)
-    Q_PROPERTY(int dopplerDist READ dopplerDist NOTIFY dopplerVeloComplete)
-
     Q_PROPERTY(QString devName READ devName NOTIFY deviceVersionChanged)
     Q_PROPERTY(int devSN READ devSerialNumber NOTIFY deviceVersionChanged)
     Q_PROPERTY(QString fwVersion READ fwVersion NOTIFY deviceVersionChanged)

DeviceManager.cpp

@@ -482,6 +482,35 @@ void DeviceManager::upgradeLastDev(QByteArray data)
     }
 }
 
+void DeviceManager::beaconActivationReceive(uint8_t id) {
+    QList<DevQProperty *> usbl_devs = getDevList(BoardUSBL);
+    if(usbl_devs.size() > 0) {
+        IDBinUsblSolution::USBLRequestBeacon ask = {};
+        usbl_devs[0]->askBeaconPosition(ask);
+    }
+}
+
+void DeviceManager::beaconDirectQueueAsk() {
+    QList<DevQProperty *> usbl_devs = getDevList(BoardUSBLBeacon);
+    qDebug("Sent request to the Beacon # %d", -1);
+    if(usbl_devs.size() > 0) {
+        usbl_devs[0]->enableBeaconOnce(3);
+        qDebug("Sent request to the Beacon # %d", 0);
+    }
+}
+
+void DeviceManager::setUSBLBeaconDirectAsk(bool is_ask) {
+    isUSBLBeaconDirectAsk = is_ask;
+    qDebug("Beacon auto scan is: %d", is_ask);
+    if(is_ask == true) {
+        QObject::connect(&beacon_timer, &QTimer::timeout, this, &DeviceManager::beaconDirectQueueAsk);
+        beacon_timer.setInterval(3000);
+        beacon_timer.start();
+    } else {
+        beacon_timer.stop();
+    }
+}
+
 #ifdef MOTOR
 float DeviceManager::getFAngle()
 {
@@ -557,14 +586,14 @@ void DeviceManager::clearTasks()
 }
 
 void DeviceManager::calibrationStandIn(float currFAngle, float taskFAngle, float currSAngle, float taskSAngle) {
+    emit encoderComplete(currFAngle, -currSAngle, NAN);
+    emit posIsConstant(currFAngle, taskFAngle, currSAngle, taskSAngle);
+
     QList<DevQProperty *> usbl_devs = getDevList(BoardUSBL);
     if(usbl_devs.size() > 0) {
-        IDBinUsblSolution::AskBeacon ask;
+        IDBinUsblSolution::USBLRequestBeacon ask;
         usbl_devs[0]->askBeaconPosition(ask);
     }
-
-    emit encoderComplete(currFAngle, currSAngle, NAN);
-    emit posIsConstant(currFAngle, taskFAngle, currSAngle, taskSAngle);
 }
 #endif
 
@@ -659,6 +688,7 @@ DevQProperty* DeviceManager::createDev(QUuid uuid, Link* link, uint8_t addr)
     connect(dev, &DevQProperty::attitudeComplete, this, &DeviceManager::attitudeComplete);
     connect(dev, &DevQProperty::distComplete, this, &DeviceManager::distComplete);
     connect(dev, &DevQProperty::usblSolutionComplete, this, &DeviceManager::usblSolutionComplete);
+    connect(dev, &DevQProperty::beaconActivationComplete, this, &DeviceManager::beaconActivationReceive);
     connect(dev, &DevQProperty::dopplerBeamComplete, this, &DeviceManager::dopplerBeamComlete);
     connect(dev, &DevQProperty::dvlSolutionComplete, this, &DeviceManager::dvlSolutionComplete);
     connect(dev, &DevQProperty::upgradeProgressChanged, this, &DeviceManager::upgradeProgressChanged);

DeviceManager.h

@@ -53,6 +53,11 @@ public slots:
     void setProtoBinConsoled(bool isConsoled);
     void upgradeLastDev(QByteArray data);
 
+    void beaconActivationReceive(uint8_t id);
+    void beaconDirectQueueAsk();
+    bool isbeaconDirectQueueAsk() { return isUSBLBeaconDirectAsk; }
+    void setUSBLBeaconDirectAsk(bool is_ask);
+
 #ifdef MOTOR
     float getFAngle();
     float getSAngle();
@@ -146,6 +151,9 @@ public slots:
     bool isConsoled_;
     volatile bool break_;
 
+    bool isUSBLBeaconDirectAsk = false;
+    QTimer beacon_timer;
+
 #ifdef MOTOR
     std::unique_ptr<MotorControl> motorControl_;
     float fAngle_ = 0.0f;

DeviceManagerWrapper.h

@@ -25,6 +25,8 @@ class DeviceManagerWrapper : public QObject
     Q_PROPERTY(int                  pilotArmState       READ    pilotArmState           NOTIFY vruChanged)
     Q_PROPERTY(int                  pilotModeState      READ    pilotModeState          NOTIFY vruChanged)
 
+    Q_PROPERTY(bool                 isbeaconDirectQueueAsk    WRITE   setUSBLBeaconDirectAsk)
+
 #ifdef MOTOR
     // MotorControl on DeviceManager
     Q_PROPERTY(int countMotorDevices READ getMotorCountDevices NOTIFY motorDeviceChanged)
@@ -56,6 +58,7 @@ class DeviceManagerWrapper : public QObject
 
     void                 setProtoBinConsoled(bool state) { getWorker()->setProtoBinConsoled(state); }
 
+    void                 setUSBLBeaconDirectAsk(bool is_ask) { getWorker()->setUSBLBeaconDirectAsk(is_ask); }
 public slots:
 
     Q_INVOKABLE bool isCreatedId(int id) { return getWorker()->isCreatedId(id); };

IDBinnary.cpp

@@ -893,31 +893,30 @@ void IDBinDVLMode::setModes(bool ismode1, bool ismode2, bool ismode3, bool ismod
 
 Resp IDBinUsblSolution::parsePayload(FrameParser &proto) {
     if(proto.ver() == v0) {
-        if(static_cast<int16_t>(sizeof(UsblSolution)) <= proto.readAvailable()) {
-            _usblSolution = proto.read<UsblSolution>();
-            qInfo("USBL d: %f, a: %f,e: %f, p: %lld", _usblSolution.distance_m, _usblSolution.azimuth_deg, _usblSolution.elevation_deg, _usblSolution.carrier_counter);
-        } else {
-            return respErrorPayload;
-        }
+        _usblSolution = proto.read<UsblSolution>();
+        qInfo("USBL d: %f, a: %f,e: %f, p: %lld", _usblSolution.distance_m, _usblSolution.azimuth_deg, _usblSolution.elevation_deg, _usblSolution.carrier_counter);
+    } else if(proto.ver() == v1) {
+        _beaconResponcel = proto.read<BeaconActivationResponce>();
+        qInfo("Beacon responce: %d", _beaconResponcel.id);
     } else {
         return respErrorVersion;
     }
 
     return respOk;
 }
 
-void IDBinUsblSolution::askBeacon(AskBeacon ask) {
+void IDBinUsblSolution::askBeacon(USBLRequestBeacon ask) {
     ProtoBinOut req_out;
     req_out.create(GETTING, Version::v0, id(), m_address);
-    req_out.write<AskBeacon>(ask);
+    req_out.write<USBLRequestBeacon>(ask);
     req_out.end();
 
     emit binFrameOut(req_out);
 }
 
 void IDBinUsblSolution::enableBeaconOnce(float timeout) {
     ProtoBinOut req_out;
-    req_out.create(SETTING, Version::v1, id(), m_address);
+    req_out.create(GETTING, Version::v1, id(), m_address);
     req_out.write<F4>(timeout);
     req_out.end();
 

IDBinnary.h

@@ -904,30 +904,43 @@ class IDBinUsblSolution : public IDBin
 
         float snr = 0;
 
-        // double latitude;
-        // double longitude;
-        // float depth;
+        float x_m = NAN;
+        float y_m = NAN;
+        double latitude_deg = NAN;
+        double longitude_deg = NAN;
+        float depth_m = NAN;
     } __attribute__((packed));
 
-    struct AskBeacon {
+    struct USBLRequestBeacon {
         uint8_t id = 0; // 0 is promisc mode
         uint8_t reserved = 0;
         uint16_t watermark = 0;
-        double latitude = NAN;
-        double longitude = NAN;
-        float external_heading = NAN;
-        float force_beacon_depth = NAN;
+        double latitude_deg = NAN;
+        double longitude_deg = NAN;
+        float external_heading_deg = NAN;
+        float force_beacon_depth_m = NAN;
+    }  __attribute__((packed));
+
+    struct BeaconActivationResponce {
+        uint8_t id = 0; // 0 is promisc mode
+        uint8_t reserved = 0;
+        uint16_t reserved1 = 0;
+    }  __attribute__((packed));
+
+    struct BeaconActivate {
+        float timeout_s = 2;
     }  __attribute__((packed));
 
     UsblSolution usblSolution() {
         return _usblSolution;
     }
 
-    void askBeacon(AskBeacon ask);
+    void askBeacon(USBLRequestBeacon ask);
     void enableBeaconOnce(float timeout);
 
 protected:
     UsblSolution _usblSolution;
+    BeaconActivationResponce _beaconResponcel;
 };
 
 

KoggerApp.pro

@@ -4,7 +4,7 @@ QT += network
 QT += qml
 
 #CONFIG += FLASHER
-# CONFIG += MOTOR # motor_control definition
+CONFIG += MOTOR # motor_control definition
 
 !android {
     QT += serialport

Link.cpp

@@ -356,17 +356,17 @@ bool Link::getIsForceStopped() const
     return isForcedStopped_;
 }
 
-#ifdef MOTOR
-void Link::setIsMotorDevice(bool isMotorDevice)
-{
-    isMotorDevice_ = isMotorDevice;
-}
-
-bool Link::getIsMotorDevice() const
-{
-    return isMotorDevice_;
-}
-#endif
+// #ifdef MOTOR
+// void Link::setIsMotorDevice(bool isMotorDevice)
+// {
+//     isMotorDevice_ = isMotorDevice;
+// }
+
+// bool Link::getIsMotorDevice() const
+// {
+//     return isMotorDevice_;
+// }
+// #endif
 
 bool Link::writeFrame(FrameParser frame)
 {
@@ -438,7 +438,7 @@ void Link::readyRead()
     QIODevice* dev = device();
     if (dev != nullptr) {
 
-        if (!isMotorDevice_) {
+        if (attribute_ == LinkAttributeNone) {
             if (linkType_ == LinkType::LinkIPUDP) {
                 QUdpSocket* socsUpd = (QUdpSocket*)dev;
                 while (socsUpd->hasPendingDatagrams()) {
@@ -458,7 +458,23 @@ void Link::readyRead()
             }
         }
         else {
-            emit dataReady(dev->readAll());
+            if (linkType_ == LinkType::LinkIPUDP) {
+                QUdpSocket* socsUpd = (QUdpSocket*)dev;
+                while (socsUpd->hasPendingDatagrams()) {
+                    QByteArray datagram;
+                    datagram.resize(socsUpd->pendingDatagramSize());
+                    QHostAddress sender;
+                    quint16 senderPort;
+                    qint64 slen = socsUpd->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
+                    if (slen == -1) {
+                        break;
+                    }
+                    emit dataReady(datagram);
+                }
+            }
+            else {
+                emit dataReady(dev->readAll());
+            }
         }
     }
 #else