Merge pull request #2733 from dogmaphobic/wifibridgeStatus

Adding (optional) link status data to the WiFi Bridge panel.

src/AutoPilotPlugins/Common/ESP8266ComponentController.h

@@ -58,9 +58,10 @@ public:
     Q_PROPERTY(QStringList      baudRates       READ baudRates                              CONSTANT)
     Q_PROPERTY(int              baudIndex       READ baudIndex      WRITE setBaudIndex      NOTIFY baudIndexChanged)
     Q_PROPERTY(bool             busy            READ busy                                   NOTIFY busyChanged)
+    Q_PROPERTY(Vehicle*         vehicle         READ vehicle        CONSTANT)
 
     Q_INVOKABLE void restoreDefaults();
-    Q_INVOKABLE void reboot();
+    Q_INVOKABLE void reboot         ();
 
     int             componentID     () { return MAV_COMP_ID_UDP_BRIDGE; }
     QString         version         ();
@@ -70,6 +71,7 @@ public:
     QStringList     baudRates       () { return _baudRates; }
     int             baudIndex       ();
     bool            busy            () { return _waitType != WAIT_FOR_NOTHING; }
+    Vehicle*        vehicle         () { return _vehicle; }
 
     void        setWifiSSID         (QString id);
     void        setWifiPassword     (QString pwd);

src/Vehicle/Vehicle.cc

@@ -111,6 +111,12 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _joystickManager(joystickManager)
     , _flowImageIndex(0)
     , _allLinksInactiveSent(false)
+    , _messagesReceived(0)
+    , _messagesSent(0)
+    , _messagesLost(0)
+    , _messageSeq(0)
+    , _compID(0)
+    , _heardFrom(false)
 {
     _addLink(link);
 
@@ -215,6 +221,16 @@ Vehicle::~Vehicle()
 
 }
 
+void
+Vehicle::resetCounters()
+{
+    _messagesReceived   = 0;
+    _messagesSent       = 0;
+    _messagesLost       = 0;
+    _messageSeq         = 0;
+    _heardFrom          = false;
+}
+
 void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
 {
     if (message.sysid != _id && message.sysid != 0) {
@@ -225,6 +241,32 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
         _addLink(link);
     }
 
+    //-- Check link status
+    _messagesReceived++;
+    emit messagesReceivedChanged();
+    if(!_heardFrom) {
+        if(message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
+            _heardFrom = true;
+            _compID = message.compid;
+            _messageSeq = message.seq + 1;
+        }
+    } else {
+        if(_compID == message.compid) {
+            uint16_t seq_received = (uint16_t)message.seq;
+            uint16_t packet_lost_count = 0;
+            //-- Account for overflow during packet loss
+            if(seq_received < _messageSeq) {
+                packet_lost_count = (seq_received + 255) - _messageSeq;
+            } else {
+                packet_lost_count = seq_received - _messageSeq;
+            }
+            _messageSeq = message.seq + 1;
+            _messagesLost += packet_lost_count;
+            if(packet_lost_count)
+                emit messagesLostChanged();
+        }
+    }
+
     // Give the plugin a change to adjust the message contents
     _firmwarePlugin->adjustMavlinkMessage(this, &message);
 
@@ -471,6 +513,8 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
     int len = mavlink_msg_to_send_buffer(buffer, &message);
 
     link->writeBytes((const char*)buffer, len);
+    _messagesSent++;
+    emit messagesSentChanged();
 }
 
 void Vehicle::_sendMessage(mavlink_message_t message)
@@ -1269,8 +1313,8 @@ void Vehicle::_connectionLostTimeout(void)
 {
     if (_connectionLostEnabled && !_connectionLost) {
         _connectionLost = true;
+        _heardFrom = false;
         emit connectionLostChanged(true);
-
         _say(QString("connection lost to vehicle %1").arg(id()), GAudioOutput::AUDIO_SEVERITY_NOTICE);
     }
 }
@@ -1278,11 +1322,9 @@ void Vehicle::_connectionLostTimeout(void)
 void Vehicle::_connectionActive(void)
 {
     _connectionLostTimer.start();
-
     if (_connectionLost) {
         _connectionLost = false;
         emit connectionLostChanged(false);
-
         _say(QString("connection regained to vehicle %1").arg(id()), GAudioOutput::AUDIO_SEVERITY_NOTICE);
     }
 }

src/Vehicle/Vehicle.h

@@ -119,6 +119,12 @@ public:
     Q_PROPERTY(bool                 genericFirmware         READ genericFirmware                        CONSTANT)
     Q_PROPERTY(bool                 connectionLost          READ connectionLost                         NOTIFY connectionLostChanged)
     Q_PROPERTY(bool                 connectionLostEnabled   READ connectionLostEnabled  WRITE setConnectionLostEnabled NOTIFY connectionLostEnabledChanged)
+    Q_PROPERTY(uint                 messagesReceived        READ messagesReceived                       NOTIFY messagesReceivedChanged)
+    Q_PROPERTY(uint                 messagesSent            READ messagesSent                           NOTIFY messagesSentChanged)
+    Q_PROPERTY(uint                 messagesLost            READ messagesLost                           NOTIFY messagesLostChanged)
+
+    /// Resets link status counters
+    Q_INVOKABLE void resetCounters  ();
 
     /// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink
     /// message. For example PX4 Flight Stack reserves the first 8 buttons to simulate rc switches.
@@ -275,6 +281,9 @@ public:
     bool            genericFirmware     () { return !px4Firmware() && !apmFirmware(); }
     bool            connectionLost      () const { return _connectionLost; }
     bool            connectionLostEnabled() const { return _connectionLostEnabled; }
+    uint            messagesReceived    () { return _messagesReceived; }
+    uint            messagesSent        () { return _messagesSent; }
+    uint            messagesLost        () { return _messagesLost; }
 
     void setConnectionLostEnabled(bool connectionLostEnabled);
 
@@ -305,6 +314,10 @@ signals:
     void connectionLostChanged(bool connectionLost);
     void connectionLostEnabledChanged(bool connectionLostEnabled);
 
+    void messagesReceivedChanged    ();
+    void messagesSentChanged        ();
+    void messagesLostChanged        ();
+
     /// Used internally to move sendMessage call to main thread
     void _sendMessageOnThread(mavlink_message_t message);
     void _sendMessageOnLinkOnThread(LinkInterface* link, mavlink_message_t message);
@@ -506,6 +519,13 @@ private:
 
     bool _allLinksInactiveSent; ///< true: allLinkInactive signal already sent one time
 
+    uint                _messagesReceived;
+    uint                _messagesSent;
+    uint                _messagesLost;
+    uint8_t             _messageSeq;
+    uint8_t             _compID;
+    bool                _heardFrom;
+
     // Settings keys
     static const char* _settingsGroup;
     static const char* _joystickModeSettingsKey;