Permalink
Browse files

Changed so that drone state is read from drone navdata

  • Loading branch information...
1 parent 2fc5cb7 commit 98f60cc248917b983040c93b8db7f4c27d5c60f7 @AndersMalmgren AndersMalmgren committed Jul 23, 2011
@@ -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);
- }
}
}
@@ -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;
}
}
@@ -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;
}
}
@@ -24,8 +24,6 @@ public class SwitchCameraCommand : Command
public SwitchCameraCommand(DroneCameraMode videoMode)
: base()
{
- outcome.Add(CommandStatusOutcome.SwitchCamera);
-
this.cameraMode = videoMode;
}
@@ -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
{
@@ -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

0 comments on commit 98f60cc

Please sign in to comment.