From 4e815a1661ebb3111b632d9f8bbde65319820be0 Mon Sep 17 00:00:00 2001
From: Nitzan Orr <nitzan.orr@gmail.com>
Date: Tue, 11 Aug 2020 10:06:53 -0700
Subject: [PATCH] add func to check drone connection status

---
 Assets/Scripts/Drone Scripts/DroneMenu.cs        |  4 ++++
 .../Drones/Matrice_ROSDroneConnection.cs         | 16 ++++++++++++++++
 .../ROS Connection Interfaces.cs                 |  7 +++++++
 3 files changed, 27 insertions(+)

diff --git a/Assets/Scripts/Drone Scripts/DroneMenu.cs b/Assets/Scripts/Drone Scripts/DroneMenu.cs
index 03b7c9269..ca122858d 100644
--- a/Assets/Scripts/Drone Scripts/DroneMenu.cs	
+++ b/Assets/Scripts/Drone Scripts/DroneMenu.cs	
@@ -35,6 +35,7 @@ public class DroneMenu : MonoBehaviour {
     private Dictionary<Text, string> infoTextsDict; // Holds all Text boxes and their topic names
     private bool initialized;
     private Transform headsetTransform;
+    private bool connection_status;
 
     /// <summary>
     /// Creates text box for each subscriber and properly displays them above the drone.
@@ -118,6 +119,9 @@ void Update() {
                 // canvas LookAt code is funky. Credit: https://answers.unity.com/questions/132592/lookat-in-opposite-direction.html
                 menuCanvas.transform.LookAt(2 * menuCanvas.transform.position - targetPosition);
             }
+
+            connection_status = connection.CheckConnectionStatus();
+            //TODO: change drone info UI to indicate connection status!
         }
     }
 
diff --git a/Assets/Scripts/ROS Scripts/ROS Connections v2/Drones/Matrice_ROSDroneConnection.cs b/Assets/Scripts/ROS Scripts/ROS Connections v2/Drones/Matrice_ROSDroneConnection.cs
index d942d6120..1279ef831 100644
--- a/Assets/Scripts/ROS Scripts/ROS Connections v2/Drones/Matrice_ROSDroneConnection.cs	
+++ b/Assets/Scripts/ROS Scripts/ROS Connections v2/Drones/Matrice_ROSDroneConnection.cs	
@@ -195,6 +195,10 @@ public bool sdk_ready
     /// </summary>
     bool home_position_set = false;
 
+    double prev_gps_msg_time = -1;
+    //double curr_gps_msg_time = -1;
+    bool connection_status = false;
+
     /// <summary>
     /// Function called by ROSManager when Drone Gameobject is initilized to start the ROS connection with requested subscribers.
     /// </summary>
@@ -865,6 +869,7 @@ public ROSBridgeMsg OnReceiveMessage(string topic, JSONNode raw_msg, ROSBridgeMs
             case "/dji_sdk/gps_position":
             case "/dji_sdk/rtk_position":
                 gps_position = (parsed == null) ? new NavSatFixMsg(raw_msg) : (NavSatFixMsg)parsed;
+                prev_gps_msg_time = DateTime.Now.TimeOfDay.TotalMilliseconds;
                 result = gps_position;
                 if (gps_position.GetLatitude() == 0.0f && gps_position.GetLongitude() == 0.0f)
                 {
@@ -897,6 +902,17 @@ public ROSBridgeMsg OnReceiveMessage(string topic, JSONNode raw_msg, ROSBridgeMs
         return result;
     }
 
+    //int prev_gps_timestamp;
+    //int curr_gps_timestamp;
+    public bool CheckConnectionStatus()
+    {
+        if (prev_gps_msg_time > -1)
+        {
+            return DateTime.Now.TimeOfDay.TotalMilliseconds - prev_gps_msg_time < 500;
+        }
+        return false;
+
+    }
     /// <summary>
     /// Get ROS message type for a valid topic.
     /// </summary>
diff --git a/Assets/Scripts/ROS Scripts/ROS Connections v2/ROS Connection Interfaces.cs b/Assets/Scripts/ROS Scripts/ROS Connections v2/ROS Connection Interfaces.cs
index 73b52e954..fb5c445f7 100644
--- a/Assets/Scripts/ROS Scripts/ROS Connections v2/ROS Connection Interfaces.cs	
+++ b/Assets/Scripts/ROS Scripts/ROS Connections v2/ROS Connection Interfaces.cs	
@@ -118,9 +118,16 @@ public interface ROSDroneConnectionInterface
     /// </summary>
     void DisconnectROSConnection();
 
+    /// <summary>
+    /// Checks whether new GPS messages are bring receivied i.e. connection status is good
+    /// </summary>
+    bool CheckConnectionStatus();
+
     // Optional in the future
     // void DoTask()
 
+
+
 }
 
 public interface ROSSensorConnectionInterface