Merge pull request #28 from AndersMalmgren/master

Current drone state is read of the drone
shtejv · Jul 31, 2011 · 79c5bc8 · 79c5bc8
2 parents d8dcdcb + 98f60cc
commit 79c5bc8
Show file tree

Hide file tree

Showing 6 changed files with 27 additions and 68 deletions.
diff --git a/ARDroneControlLibrary/Commands/Command.cs b/ARDroneControlLibrary/Commands/Command.cs
@@ -29,28 +29,15 @@ public enum CommandStatusPrerequisite
         NotEmergency
     }
 
-    public enum CommandStatusOutcome
-    {
-        SetFlying,
-        ClearFlying,
-        SetHovering,
-        ClearHovering,
-        SetEmergency,
-        ClearEmergency,
-        SwitchCamera
-    }
-
     public abstract class Command
     {
         protected int sequenceNumber = -1;
 
         protected HashSet<CommandStatusPrerequisite> prerequisites;
-        protected HashSet<CommandStatusOutcome> outcome;
 
         protected Command()
         {
             prerequisites = new HashSet<CommandStatusPrerequisite>();
-            outcome = new HashSet<CommandStatusOutcome>();
 
             prerequisites.Add(CommandStatusPrerequisite.Connected);
         }
@@ -75,10 +62,5 @@ public bool NeedsPrerequisite(CommandStatusPrerequisite prerequisiteEntry)
         {
             return prerequisites.Contains(prerequisiteEntry);
         }
-
-        public bool HasOutcome(CommandStatusOutcome outcomeEntry)
-        {
-            return outcome.Contains(outcomeEntry);
-        }
     }
 }
diff --git a/ARDroneControlLibrary/Commands/FlightModeCommand.cs b/ARDroneControlLibrary/Commands/FlightModeCommand.cs
@@ -44,21 +44,10 @@ private void SetPrerequisitesAndOutcome()
                 case DroneFlightMode.TakeOff:
                     prerequisites.Add(CommandStatusPrerequisite.NotFlying);
                     prerequisites.Add(CommandStatusPrerequisite.NotEmergency);
-                    outcome.Add(CommandStatusOutcome.SetFlying);
                     break;
                 case DroneFlightMode.Land:
                     prerequisites.Add(CommandStatusPrerequisite.Flying);
                     prerequisites.Add(CommandStatusPrerequisite.NotEmergency);
-                    outcome.Add(CommandStatusOutcome.ClearFlying);
-                    outcome.Add(CommandStatusOutcome.ClearHovering);
-                    break;
-                case DroneFlightMode.Emergency:
-                    outcome.Add(CommandStatusOutcome.ClearFlying);
-                    outcome.Add(CommandStatusOutcome.ClearHovering);
-                    outcome.Add(CommandStatusOutcome.SetEmergency);
-                    break;
-                case DroneFlightMode.Reset:
-                    outcome.Add(CommandStatusOutcome.ClearEmergency);
                     break;
             }
         }

diff --git a/ARDroneControlLibrary/Commands/HoverModeCommand.cs b/ARDroneControlLibrary/Commands/HoverModeCommand.cs
@@ -43,13 +43,11 @@ private void SetPrerequisitesAndOutcome()
                     prerequisites.Add(CommandStatusPrerequisite.Flying);
                     prerequisites.Add(CommandStatusPrerequisite.NotEmergency);
                     prerequisites.Add(CommandStatusPrerequisite.NotHovering);
-                    outcome.Add(CommandStatusOutcome.SetHovering);
                     break;
                 case DroneHoverMode.StopHovering:
                     prerequisites.Add(CommandStatusPrerequisite.Flying);
                     prerequisites.Add(CommandStatusPrerequisite.NotEmergency);
                     prerequisites.Add(CommandStatusPrerequisite.Hovering);
-                    outcome.Add(CommandStatusOutcome.ClearHovering);
                     break;
             }
         }

diff --git a/ARDroneControlLibrary/Commands/SwitchCameraCommand.cs b/ARDroneControlLibrary/Commands/SwitchCameraCommand.cs
@@ -24,8 +24,6 @@ public class SwitchCameraCommand : Command
         public SwitchCameraCommand(DroneCameraMode videoMode)
             : base()
         {
-            outcome.Add(CommandStatusOutcome.SwitchCamera);
-
             this.cameraMode = videoMode;
         }
 

diff --git a/ARDroneControlLibrary/Data/NavigationData.cs b/ARDroneControlLibrary/Data/NavigationData.cs
@@ -42,6 +42,19 @@ public struct NavigationDataStruct
         public Single VX;
         public Single VY;
         public Single VZ;
+    }
+
+    public enum DroneState
+    {
+        Emergency = 0,
+        Init = 1,
+        Landed = 2,
+        Flying = 3,
+        Hovering = 4,
+        Test = 5,
+        TransTakeoff = 6,
+        TransGotoFix = 7,
+        Landing = 8
     }
 
     public class DroneData
@@ -72,7 +85,11 @@ public DroneData()
         }
 
         public DroneData(NavigationDataStruct navigationDataStruct)
-        {
+        {
+            uint major = navigationDataStruct.ControlStatus >> 16;
+            //uint minor = navigationDataStruct.ControlStatus & 0xFFFF;
+            DroneState = (DroneState)major;
+
             phi = navigationDataStruct.Phi / 1000.0;
             psi = navigationDataStruct.Psi / 1000.0;
             theta = navigationDataStruct.Theta / 1000.0;
@@ -83,7 +100,9 @@ public DroneData(NavigationDataStruct navigationDataStruct)
 
             altitude = navigationDataStruct.Altitude;
             batteryLevel = (int)navigationDataStruct.BatteryLevel;
-        }
+        }
+
+        public DroneState DroneState { get; private set; }
 
         public double Phi
         {

diff --git a/ARDroneControlLibrary/DroneControl.cs b/ARDroneControlLibrary/DroneControl.cs
@@ -80,10 +80,6 @@ public class DroneControl : IDroneControl
 
         private bool connecting = false;
         private bool connectToBothNetworkAndDrone = false;
-
-        private bool flying = false;
-        private bool hovering = false;
-        private bool emergency = false;
 
         private CheckFlightMoveCommandStrategy checkFlightMoveCommandStrategy;
 
@@ -264,8 +260,6 @@ private void ConnectWorkers()
                 controlInfoRetriever.Connect();
             if (!commandSender.Connected)
                 commandSender.Connect();
-
-            ResetFlightVariables();
         }
 
         public void Disconnect()
@@ -278,15 +272,6 @@ public void Disconnect()
                 navigationDataRetriever.Disconnect();
             if (videoDataRetriever.Connected)
                 videoDataRetriever.Disconnect();
-
-            ResetFlightVariables();
-        }
-
-        private void ResetFlightVariables()
-        {
-            flying = false;
-            hovering = false;
-            emergency = false;
         }
 
         public void SendCommand(Command command)
@@ -296,7 +281,6 @@ public void SendCommand(Command command)
             if (IsCommandPossible(command) && CheckFlightMoveCommand(command))
             {
                 commandSender.SendQueuedCommand(command);
-                ChangeStatusAccordingToCommand(command);
             }
         }
 
@@ -350,19 +334,7 @@ private bool CheckFlightMoveCommand(Command command)
         {
             return checkFlightMoveCommandStrategy.Check(command);
         }
-
-        private void ChangeStatusAccordingToCommand(Command command)
-        {
-            if (command.HasOutcome(CommandStatusOutcome.SetFlying)) flying = true;
-            if (command.HasOutcome(CommandStatusOutcome.ClearFlying)) flying = false;
-
-            if (command.HasOutcome(CommandStatusOutcome.SetHovering)) hovering = true;
-            if (command.HasOutcome(CommandStatusOutcome.ClearHovering)) hovering = false;
-
-            if (command.HasOutcome(CommandStatusOutcome.SetEmergency)) emergency = true;
-            if (command.HasOutcome(CommandStatusOutcome.ClearEmergency)) emergency = false;
-        }
-
+
         public Bitmap BitmapImage
         {
             get { return videoDataRetriever.CurrentBitmap; }
@@ -406,10 +378,11 @@ private void networkWorker_Error(object sender, NetworkWorkerErrorEventArgs e)
         // Current drone state
 
         public bool IsConnecting { get { return connecting; } }
-        public bool IsConnected { get { return videoDataRetriever.Connected && navigationDataRetriever.Connected && commandSender.Connected; } }
-        public bool IsFlying { get { return flying; } }
-        public bool IsHovering { get { return hovering; } }
-        public bool IsEmergency { get { return emergency; } }
+        public bool IsConnected { get { return videoDataRetriever.Connected && navigationDataRetriever.Connected && commandSender.Connected; } }
+        public bool IsFlying { get { return NavigationData.DroneState == DroneState.Flying || NavigationData.DroneState == DroneState.Hovering || NavigationData.DroneState == DroneState.TransGotoFix; } }
+        public bool IsHovering { get { return NavigationData.DroneState == DroneState.Hovering; } }
+        public bool IsEmergency { get { return NavigationData.DroneState == DroneState.Emergency; } }
+
         public DroneCameraMode CurrentCameraType { get { return currentCameraMode; } }
 
         // Current drone capabilities