Moved code in method 'CheckFlightMoveCommand' to a seperate class, th…

…is is for testabilty and flexability. Also fixed a bug with hoovercommand not reseting last values.
theone1984 · Jul 21, 2011 · 2fc5cb7 · 2fc5cb7
1 parent 8970e3a
commit 2fc5cb7
Show file tree

Hide file tree

Showing 3 changed files with 152 additions and 126 deletions.
diff --git a/ARDroneControlLibrary/ARDroneControlLibrary.csproj b/ARDroneControlLibrary/ARDroneControlLibrary.csproj
@@ -1,98 +1,99 @@
-<?xml version="1.0" encoding="utf-8"?>
-<Project ToolsVersion="4.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
-  <PropertyGroup>
-    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
-    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
-    <ProductVersion>8.0.30703</ProductVersion>
-    <SchemaVersion>2.0</SchemaVersion>
-    <ProjectGuid>{92277DFC-0D55-4998-8ACF-B01F3A7FB508}</ProjectGuid>
-    <OutputType>Library</OutputType>
-    <AppDesignerFolder>Properties</AppDesignerFolder>
-    <RootNamespace>ARDrone.Control</RootNamespace>
-    <AssemblyName>ARDrone.Control</AssemblyName>
-    <TargetFrameworkVersion>v4.0</TargetFrameworkVersion>
-    <FileAlignment>512</FileAlignment>
-  </PropertyGroup>
-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
-    <DebugSymbols>true</DebugSymbols>
-    <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
-    <OutputPath>..\bin\Debug\ARDroneControlLibrary\</OutputPath>
-    <DefineConstants>DEBUG;TRACE</DefineConstants>
-    <ErrorReport>prompt</ErrorReport>
-    <WarningLevel>4</WarningLevel>
-    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
-  </PropertyGroup>
-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
-    <DebugType>pdbonly</DebugType>
-    <Optimize>true</Optimize>
-    <OutputPath>..\bin\Debug\ARDroneControlLibrary\</OutputPath>
-    <DefineConstants>TRACE</DefineConstants>
-    <ErrorReport>prompt</ErrorReport>
-    <WarningLevel>4</WarningLevel>
-    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
-  </PropertyGroup>
-  <ItemGroup>
-    <Reference Include="PresentationCore" />
-    <Reference Include="System" />
-    <Reference Include="System.Core" />
-    <Reference Include="System.Drawing" />
-    <Reference Include="System.Xml.Linq" />
-    <Reference Include="System.Data.DataSetExtensions" />
-    <Reference Include="Microsoft.CSharp" />
-    <Reference Include="System.Data" />
-    <Reference Include="System.Xml" />
-    <Reference Include="WindowsBase" />
-  </ItemGroup>
-  <ItemGroup>
-    <Compile Include="Commands\FlatTrimCommand.cs" />
-    <Compile Include="Commands\FlightModeCommand.cs" />
-    <Compile Include="Commands\Command.cs" />
-    <Compile Include="Commands\FlightMoveCommand.cs" />
-    <Compile Include="Commands\HoverModeCommand.cs" />
-    <Compile Include="Commands\PlayLedAnimationCommand.cs" />
-    <Compile Include="Commands\SetConfigurationCommand.cs" />
-    <Compile Include="Commands\SetControlModeCommand.cs" />
-    <Compile Include="Commands\SwitchCameraCommand.cs" />
-    <Compile Include="Commands\WatchDogCommand.cs" />
-    <Compile Include="Data\Firmware.cs" />
-    <Compile Include="Data\InternalDroneConfiguration.cs" />
-    <Compile Include="Data\NavigationData.cs" />
-    <Compile Include="DroneConfig.cs" />
-    <Compile Include="DroneControl.cs" />
-    <Compile Include="Events\DroneEvents.cs" />
-    <Compile Include="Events\DroneExceptions.cs" />
-    <Compile Include="Events\NetworkWorkerEvents.cs" />
-    <Compile Include="Network\KeepAliveNetworkWorker.cs" />
-    <Compile Include="Network\NetworkWorker.cs" />
-    <Compile Include="Network\TcpWorker.cs" />
-    <Compile Include="Network\UdpWorker.cs" />
-    <Compile Include="Network\BackgroundWorker.cs" />
-    <Compile Include="Properties\AssemblyInfo.cs" />
-    <Compile Include="Utils\BitmapUtils.cs" />
-    <Compile Include="Utils\NetworkUtils.cs" />
-    <Compile Include="Utils\VideoUtils.cs" />
-    <Compile Include="Wifi\WifiInterop.cs" />
-    <Compile Include="Wifi\WlanApi.cs" />
-    <Compile Include="Workers\NetworkConnector.cs" />
-    <Compile Include="Workers\CommandSender.cs" />
-    <Compile Include="Workers\ControlInfoRetriever.cs" />
-    <Compile Include="Workers\NavigationDataRetriever.cs" />
-    <Compile Include="Workers\NetworkSanityChecker.cs" />
-    <Compile Include="Workers\VideoDataRetriever.cs" />
-  </ItemGroup>
-  <ItemGroup>
-    <ProjectReference Include="..\ARDroneBasics\ARDroneBasics.csproj">
-      <Project>{30FB7E9C-2668-4303-BF0D-1618DD30B8E5}</Project>
-      <Name>ARDroneBasics</Name>
-    </ProjectReference>
-  </ItemGroup>
-  <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="4.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+    <ProductVersion>8.0.30703</ProductVersion>
+    <SchemaVersion>2.0</SchemaVersion>
+    <ProjectGuid>{92277DFC-0D55-4998-8ACF-B01F3A7FB508}</ProjectGuid>
+    <OutputType>Library</OutputType>
+    <AppDesignerFolder>Properties</AppDesignerFolder>
+    <RootNamespace>ARDrone.Control</RootNamespace>
+    <AssemblyName>ARDrone.Control</AssemblyName>
+    <TargetFrameworkVersion>v4.0</TargetFrameworkVersion>
+    <FileAlignment>512</FileAlignment>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>..\bin\Debug\ARDroneControlLibrary\</OutputPath>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+    <DebugType>pdbonly</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>..\bin\Debug\ARDroneControlLibrary\</OutputPath>
+    <DefineConstants>TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+  </PropertyGroup>
+  <ItemGroup>
+    <Reference Include="PresentationCore" />
+    <Reference Include="System" />
+    <Reference Include="System.Core" />
+    <Reference Include="System.Drawing" />
+    <Reference Include="System.Xml.Linq" />
+    <Reference Include="System.Data.DataSetExtensions" />
+    <Reference Include="Microsoft.CSharp" />
+    <Reference Include="System.Data" />
+    <Reference Include="System.Xml" />
+    <Reference Include="WindowsBase" />
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="CheckFlightMoveCommandStrategy.cs" />
+    <Compile Include="Commands\FlatTrimCommand.cs" />
+    <Compile Include="Commands\FlightModeCommand.cs" />
+    <Compile Include="Commands\Command.cs" />
+    <Compile Include="Commands\FlightMoveCommand.cs" />
+    <Compile Include="Commands\HoverModeCommand.cs" />
+    <Compile Include="Commands\PlayLedAnimationCommand.cs" />
+    <Compile Include="Commands\SetConfigurationCommand.cs" />
+    <Compile Include="Commands\SetControlModeCommand.cs" />
+    <Compile Include="Commands\SwitchCameraCommand.cs" />
+    <Compile Include="Commands\WatchDogCommand.cs" />
+    <Compile Include="Data\Firmware.cs" />
+    <Compile Include="Data\InternalDroneConfiguration.cs" />
+    <Compile Include="Data\NavigationData.cs" />
+    <Compile Include="DroneConfig.cs" />
+    <Compile Include="DroneControl.cs" />
+    <Compile Include="Events\DroneEvents.cs" />
+    <Compile Include="Events\DroneExceptions.cs" />
+    <Compile Include="Events\NetworkWorkerEvents.cs" />
+    <Compile Include="Network\KeepAliveNetworkWorker.cs" />
+    <Compile Include="Network\NetworkWorker.cs" />
+    <Compile Include="Network\TcpWorker.cs" />
+    <Compile Include="Network\UdpWorker.cs" />
+    <Compile Include="Network\BackgroundWorker.cs" />
+    <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="Utils\BitmapUtils.cs" />
+    <Compile Include="Utils\NetworkUtils.cs" />
+    <Compile Include="Utils\VideoUtils.cs" />
+    <Compile Include="Wifi\WifiInterop.cs" />
+    <Compile Include="Wifi\WlanApi.cs" />
+    <Compile Include="Workers\NetworkConnector.cs" />
+    <Compile Include="Workers\CommandSender.cs" />
+    <Compile Include="Workers\ControlInfoRetriever.cs" />
+    <Compile Include="Workers\NavigationDataRetriever.cs" />
+    <Compile Include="Workers\NetworkSanityChecker.cs" />
+    <Compile Include="Workers\VideoDataRetriever.cs" />
+  </ItemGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\ARDroneBasics\ARDroneBasics.csproj">
+      <Project>{30FB7E9C-2668-4303-BF0D-1618DD30B8E5}</Project>
+      <Name>ARDroneBasics</Name>
+    </ProjectReference>
+  </ItemGroup>
+  <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
   <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 
        Other similar extension points exist, see Microsoft.Common.targets.
   <Target Name="BeforeBuild">
   </Target>
   <Target Name="AfterBuild">
   </Target>
-  -->
+  -->
 </Project>
diff --git a/ARDroneControlLibrary/CheckFlightMoveCommandStrategy.cs b/ARDroneControlLibrary/CheckFlightMoveCommandStrategy.cs
@@ -0,0 +1,53 @@
+using System;
+using ARDrone.Control.Commands;
+
+namespace ARDrone.Control
+{
+    public class CheckFlightMoveCommandStrategy
+    {
+        private const float thresholdBetweenSettingCommands = 0.03f;
+        private float lastRollValue = 0.0f;
+        private float lastPitchValue = 0.0f;
+        private float lastGazValue = 0.0f;
+        private float lastYawValue = 0.0f;
+
+        public bool Check(Command command)
+        {
+            if (!(command is FlightMoveCommand) && !(command is HoverModeCommand))
+                return true;
+
+            if(command is HoverModeCommand)
+            {
+                lastRollValue = 0;
+                lastPitchValue = 0;
+                lastYawValue = 0;
+                lastGazValue = 0;
+
+                return true;
+            }
+
+            var moveCommand = (FlightMoveCommand)command;
+
+            if (Math.Abs(moveCommand.Roll - lastRollValue) >= thresholdBetweenSettingCommands ||
+                Math.Abs(moveCommand.Pitch - lastPitchValue) >= thresholdBetweenSettingCommands ||
+                Math.Abs(moveCommand.Yaw - lastYawValue) >= thresholdBetweenSettingCommands ||
+                Math.Abs(moveCommand.Gaz - lastGazValue) >= thresholdBetweenSettingCommands)
+            {
+                lastRollValue = moveCommand.Roll;
+                lastPitchValue = moveCommand.Pitch;
+                lastYawValue = moveCommand.Yaw;
+                lastGazValue = moveCommand.Gaz;
+                return true;
+            }
+            else if (moveCommand.Roll == 0.0f && moveCommand.Pitch == 0.0f &&
+                     moveCommand.Yaw == 0.0f && moveCommand.Gaz == 0.0f)
+            {
+                return true;
+            }
+            else
+            {
+                return false;
+            }
+        }
+    }
+}
diff --git a/ARDroneControlLibrary/DroneControl.cs b/ARDroneControlLibrary/DroneControl.cs
@@ -63,8 +63,6 @@ public interface IDroneControl
 
     public class DroneControl : IDroneControl
     {
-        private const float thresholdBetweenSettingCommands = 0.03f;
-
         // Workers
 
         private NetworkConnector networkConnector;
@@ -85,12 +83,9 @@ public class DroneControl : IDroneControl
 
         private bool flying = false;
         private bool hovering = false;
-        private bool emergency = false;
-
-        private float lastRollValue = 0.0f;
-        private float lastPitchValue = 0.0f;
-        private float lastGazValue = 0.0f;
-        private float lastYawValue = 0.0f;
+        private bool emergency = false;
+
+        private CheckFlightMoveCommandStrategy checkFlightMoveCommandStrategy;
 
         public bool lastConnectionState;
 
@@ -108,7 +103,8 @@ public class DroneControl : IDroneControl
         public event DroneNetworkConnectionStateChangedEventHandler NetworkConnectionStateChanged;
 
         public void Init(DroneConfig droneConfig)
-        {
+        {
+            checkFlightMoveCommandStrategy = new CheckFlightMoveCommandStrategy();
             this.droneConfig = droneConfig;
             droneConfig.Initialize();
 
@@ -351,32 +347,8 @@ public bool IsCommandPossible(Command command)
         }
 
         private bool CheckFlightMoveCommand(Command command)
-        {
-            if (!(command is FlightMoveCommand))
-                return true;
-
-            FlightMoveCommand moveCommand = (FlightMoveCommand)command;
-
-            if (Math.Abs(moveCommand.Roll - lastRollValue) >= thresholdBetweenSettingCommands ||
-                Math.Abs(moveCommand.Pitch - lastPitchValue) >= thresholdBetweenSettingCommands ||
-                Math.Abs(moveCommand.Yaw - lastYawValue) >= thresholdBetweenSettingCommands ||
-                Math.Abs(moveCommand.Gaz - lastGazValue) >= thresholdBetweenSettingCommands)
-            {
-                lastRollValue = moveCommand.Roll;
-                lastPitchValue = moveCommand.Pitch;
-                lastYawValue = moveCommand.Yaw;
-                lastGazValue = moveCommand.Gaz;
-                return true;
-            }
-            else if (moveCommand.Roll == 0.0f && moveCommand.Pitch == 0.0f &&
-                     moveCommand.Yaw == 0.0f && moveCommand.Gaz == 0.0f)
-            {
-                return true;
-            }
-            else
-            {
-                return false;
-            }
+        {
+            return checkFlightMoveCommandStrategy.Check(command);
         }
 
         private void ChangeStatusAccordingToCommand(Command command)