2D模拟简单灯光

3D模拟移动和简单灯光和观测点
添加校准指南针
修改摆放角度
修改模拟飞行中起飞的方案
通信模块：多条mavlink指令采用一起发送的方式

Plane.FormationCreator/Formation/CopterManager.cs

@@ -19,18 +19,20 @@ namespace Plane.FormationCreator.Formation
         /// <param name="baseSemObjects"></param>  
         public int AddCopter(ICopter entityObject)
         {
-            DateTime dateTime2019 = DateTime.Parse("2019-06-20");
 
-            if (DateTime.UtcNow > dateTime2019)
+            ////给第三方时候限制数量和时间用
-            {
+//             DateTime dateTime2019 = DateTime.Parse("2019-06-20");
-                throw new NotImplementedException();
+// 
-            }
+//             if (DateTime.UtcNow > dateTime2019)
-
+//             {
-
+//                 throw new NotImplementedException();
-            if (this.Count >= 50)
+//             }
-            {
+// 
-                return 0;
+// 
-            }
+//             if (this.Count >= 50)
+//             {
+//                 return 0;
+//             }
 
             int _index = 0;
             if (this.Count == 0)

Plane.FormationCreator/Formation/FlightTask.cs

@@ -156,7 +156,8 @@ namespace Plane.FormationCreator.Formation
                     break;
                 case FlightTaskType.TakeOff:
                     //多架同时起飞
-                    await MutilRunTakeOffTaskAsync().ConfigureAwait(false);
+                    //await MutilRunTakeOffTaskAsync().ConfigureAwait(false);
+                    await NewMutilRunTakeOffTaskAsync().ConfigureAwait(false);
                     break;
                 case FlightTaskType.ReturnToLand:   // Added by ZJF
                     //多架同时返航

Plane.FormationCreator/Formation/FlightTaskManager.cs

@@ -262,6 +262,7 @@ namespace Plane.FormationCreator.Formation
                 float coldis = ColumnDistance;//列相距5米
                 float rowdis = RowDistance;//行相距5米
                 float matrixdis = 20; //生成方阵距离30米
+                int orientation = Orientation;
 
                 int currcol = 0; //当前列号
                 int currrow = 0; //当前行
@@ -276,18 +277,18 @@ namespace Plane.FormationCreator.Formation
                     if (coptindex == 0)
                     {
                         lastSingleCopterInfo = lastTask.SingleCopterInfos.Find(info => info.Copter == copter);
-                        targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(lastSingleCopterInfo.TargetLat, lastSingleCopterInfo.TargetLng, 0, matrixdis);
+                        targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(lastSingleCopterInfo.TargetLat, lastSingleCopterInfo.TargetLng, orientation, matrixdis);
                         colLatLng = targetLatLng;
                     }
                     else
                     {
                         if (currcol < colnum)
-                            targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Item1, colLatLng.Item2, 90, currcol * coldis);
+                            targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Item1, colLatLng.Item2, orientation + 90, currcol * coldis);
                         else
                         {
                             currrow++;
                             currcol = 0;
-                            targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Item1, colLatLng.Item2, 180, rowdis);
+                            targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Item1, colLatLng.Item2, orientation + 180, rowdis);
                             colLatLng = targetLatLng;
                         }
                     }

Plane.FormationCreator/Formation/FlightTaskSingleCopterInfo_LED.cs

@@ -82,6 +82,8 @@ namespace Plane.FormationCreator.Formation
             }
         }
 
+
+
         private ICommand _SetSelectedCopterLEDCommand;
         public ICommand SetSelectedCopterLEDCommand
         {

Plane.FormationCreator/Formation/FlightTask_FlyTo.cs

@@ -122,14 +122,15 @@ namespace Plane.FormationCreator.Formation
             DateTime dtNow = DateTime.Now;
             DateTime dtLastTime = DateTime.Now;
             TimeSpan ts = dtNow - dtLastTime;
+            
 
             FlightTask task = _flightTaskManager.CurrentRunningTask;
             int flyToTime = task.FlytoTime * 1000;
             int loiterTime = task.LoiterTime * 1000;
             int taskIndex = _flightTaskManager.CurrentRunningTaskIndex;
             int copterIndex = SingleCopterInfos.IndexOf(info);
+     
 
-         
             // 当该飞机被标记时，悬停并跳过飞行任务
             if ((bool)_copterManager.CopterStatus[copterIndex])
             {
@@ -184,9 +185,29 @@ namespace Plane.FormationCreator.Formation
                     await info.Copter.HoverAsync();
                     return;
                 }
-                await Task.Delay(100).ConfigureAwait(false); //判断是否到达位置10hz
+                await Task.Delay(10).ConfigureAwait(false); //判断是否到达位置10hz
 
-                if (ts.TotalMilliseconds / 500 > sendFlyToTimes)   // 每500ms发送一遍指点坐标
+
+               
+                if (info.LEDInfos.Count > 0)
+                {
+                    string LEDRGB = "";
+                    List<LEDInfo> LedControl = info.LEDInfos.OrderBy(i=>i.Delay).ToList();
+                    for (int i = 0; i < LedControl.Count; i++)
+                    {
+                        var led = LedControl[i];
+                        if (ts.TotalMilliseconds >= led.Delay * 1000)
+                            LEDRGB = info.LEDInfos[i].LEDRGB;
+                        else
+                            break;
+                    }
+                    info.Copter.LEDColor = LEDRGB;
+
+                }
+
+               
+
+                if (ts.TotalMilliseconds / 10 > sendFlyToTimes)   // 每500ms发送一遍指点坐标
                 {
                     sendFlyToTimes++;
                     for (int i = 0; i < 2; i++)

Plane.FormationCreator/Formation/FlightTask_TakeOff.cs

@@ -69,6 +69,80 @@ namespace Plane.FormationCreator.Formation
             }
         }
 
+        //新版的起飞方案
+        public async Task NewMutilRunTakeOffTaskAsync()
+        {
+            var infos = SingleCopterInfos;
+            var tasksTakeOff = new Task[infos.Count];
+            for (int i = 0; i < infos.Count; i++)
+            {
+                tasksTakeOff[i] = NewSingleRunTaskOffTaskAsunc(infos[i]);
+            }
+            await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
+            //await Task.Delay(100);
+        }
+
+        private async Task NewSingleRunTaskOffTaskAsunc(FlightTaskSingleCopterInfo info)
+        {
+            DateTime dtNow = DateTime.Now;
+            DateTime dtLastTime = DateTime.Now;
+            TimeSpan ts = dtNow - dtLastTime;
+
+            int copterIndex = SingleCopterInfos.IndexOf(info);
+            var copter = info.Copter;
+            
+            await copter.UnlockAsync();
+            for (int i = 0; !copter.IsUnlocked; i++)
+            {
+                //8秒内每1000毫秒尝试解锁一次
+                //解锁间隔一定要超过1s否则导致飞控以后无法解锁
+
+                if (i > 320)
+                    return;  //无法解锁后面不用执行了
+                if (i % (1000 / 25) == 1000 / 25 - 1)
+                {
+                    await copter.UnlockAsync(); // 每 1000 毫秒重试一次。
+                }
+
+                await Task.Delay(25).ConfigureAwait(false);
+            }
+
+            //等待起飞时间
+            while (ts.TotalMilliseconds < info.TakeOffWaitTime * 1000)
+            {
+                await Task.Delay(100);
+                dtNow = DateTime.Now;
+                ts = dtNow - dtLastTime;
+            }
+
+            for (int i = 0; i < 5; i++)   // added by ZJF
+            {
+                await copter.TakeOffAsync();
+                await Task.Delay(50).ConfigureAwait(false);
+            }
+
+            var copterNextTask = _flightTaskManager.Tasks[TaskIndex + 1].SingleCopterInfos[copterIndex];
+            float takeOffAlt = copterNextTask.TargetAlt;
+
+            for (int j = 0; j < 3; j++)
+            {
+                await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt);
+                await Task.Delay(10).ConfigureAwait(false);
+            }
+
+            dtNow = DateTime.Now;
+            ts = dtNow - dtLastTime;
+            FlightTask task = _flightTaskManager.CurrentRunningTask;
+            while (ts.TotalMilliseconds < task.TakeOffTime * 1000)
+            {
+                await Task.Delay(100).ConfigureAwait(false);
+                dtNow = DateTime.Now;
+                ts = dtNow - dtLastTime;
+            }
+        }
+
+
+        //老方案 ----- 按起飞数量起飞
         // 几架飞机同时起飞，参数为takeOffCount-----------------使用中
         //起飞分三个阶段：
         //1阶段：分批起飞到15米(目前15米高度是飞控起飞航点决定)，上一批起飞超过5米下一批开始起飞，
@@ -81,12 +155,14 @@ namespace Plane.FormationCreator.Formation
         //2.直接飞往第一个航点的高度
         public async Task MutilRunTakeOffTaskAsync()
         {
+            
             int TaskCount = _flightTaskManager.Tasks.Count();
             if (TaskCount > 1)
             {
                 var infos = SingleCopterInfos;
 
-                int takeOffCount = TakeOffNumAttr;
+                //不再使用起飞数量  强制设置起飞总数等于所有飞机 
+                int takeOffCount = _copterManager.Copters.Count();
                 int copterCount = infos.Count;
                 int integerPart = copterCount / takeOffCount;
                 int residualPart = copterCount % takeOffCount;
@@ -167,6 +243,10 @@ namespace Plane.FormationCreator.Formation
         //执行第一阶段解锁起飞任务---使用中
         private async Task TakeOffTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
         {
+            DateTime dtNow = DateTime.Now;
+            DateTime dtLastTime = DateTime.Now;
+            TimeSpan ts = dtNow - dtLastTime;
+
             int copterIndex = SingleCopterInfos.IndexOf(info);
             var copter = info.Copter;
             var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex];
@@ -208,6 +288,14 @@ namespace Plane.FormationCreator.Formation
             info.TargetLat = info.Copter.Latitude;
             info.TargetLng = info.Copter.Longitude;
 
+            //等待起飞时间
+            while (ts.TotalMilliseconds < info.TakeOffWaitTime * 1000 )
+            {
+                await Task.Delay(100);
+                dtNow = DateTime.Now;
+                ts = dtNow - dtLastTime;
+            }
+
             //开始起飞
             for (int i = 0; i < 5; i++)   // added by ZJF
             {

Plane.FormationCreator/Formation/MapManager.cs

@@ -1,4 +1,5 @@
-using System;
+using Plane.Copters;
+using System;
 using System.Collections.Generic;
 using System.Linq;
 using System.Text;
@@ -25,5 +26,7 @@ namespace Plane.FormationCreator.Formation
         {
             this.MapView.ClearCopters();
         }
+
+      
     }
 }

Plane.FormationCreator/MainWindow.xaml

@@ -42,14 +42,14 @@
 
     <c:MetroWindow.RightWindowCommands>
         <c:WindowCommands>
-            <Button Content="切换地图" Visibility="Collapsed" Command="{Binding ChangeMapModeCommand}"></Button>
+            <Button Content="切换地图"  Command="{Binding ChangeMapModeCommand}"></Button>
             <Button Content="{Binding AppEx.ShowModifyTaskView, Converter={StaticResource ShowModifyTaskViewButtonContentConverter}}"
                     Command="{Binding ShowOrHideModifyTaskViewCommand}" />
             <Button Content="重启监听"
                     Visibility="Collapsed"
                     Command="{Binding RestartListeningCommand}" />
             <Button Content="{Binding SwitchVelocityModeButtonContent}"
-                    Visibility="Collapsed"
+                    
                     Command="{Binding SwitchVelocityModeCommand}" />
             <!--// 林俊清, 20150930, 不分这些模式了。
             <Button Content="进入任务模式"
@@ -60,18 +60,8 @@
                     Command="{Binding SwitchAppModeCommand}"
                     CommandParameter="{x:Static m:AppMode.ControllingCopters}"
                     Visibility="{Binding Source={x:Static local:AppEx.Current}, Path=AppMode, Converter={StaticResource AppModeToVisibilityConverter}, ConverterParameter=SwitchToControllingCoptersModeButton}" />-->
-            <Button Name="btnShowLog"
-                    Content="日志"
-                    Visibility="Collapsed"
-                    Click="btnShowLog_Click" 
-                    />
-            <Button Content="地图透明"
-                    Visibility="Collapsed"
-                    Click="Map_Opacity_Click" 
-                    />
             <Button Name="btnGoHome"
                     Content="回家"
-                    Visibility="Collapsed"
                     Click="btnGoHome_Click" />
             <Button Name="btnRefreshMap"
                     Content="刷新地图"
@@ -139,7 +129,7 @@
                     <RowDefinition Height="Auto" />
                 </Grid.RowDefinitions>
 
-<!--
+
                 <TabControl SelectedIndex="{Binding MapMode,UpdateSourceTrigger=PropertyChanged}">
                     <TabItem Visibility="Collapsed">
                         <v:MapView x:Name="map"/>
@@ -148,10 +138,11 @@
                         <v:View3D/>
                     </TabItem>
                 </TabControl>
--->
+
-             
+
-                
+                <!--
                 <v:MapView x:Name="map"/>
+                -->
                
               
 

Plane.FormationCreator/MainWindow.xaml.cs

@@ -107,7 +107,7 @@ namespace Plane.FormationCreator
             switch (e.Key)
             {
 
-                case Key.LeftShift:
+                case Key.LeftCtrl:
                    {
                         var copters = _copterManager.AcceptingControlCopters;
                         Shiftkeydown = true;
@@ -391,7 +391,7 @@ namespace Plane.FormationCreator
 
             switch (e.Key)
             {
-                case Key.LeftShift:
+                case Key.LeftCtrl:
                     {
                         Shiftkeydown = false;
                         _copterManager.shiftkeydown = false;

Plane.FormationCreator/ModifyParam.xaml

@@ -5,7 +5,7 @@
         xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"
         xmlns:local="clr-namespace:Plane.FormationCreator"
         mc:Ignorable="d"
-        Title="修改参数" Height="242.765" Width="618.987" ResizeMode="NoResize" WindowStartupLocation="CenterScreen">
+        Title="修改参数" Height="418.215" Width="715.206" ResizeMode="NoResize" WindowStartupLocation="CenterScreen">
     <Grid Margin="0,0,5,3.5">
         <Grid.ColumnDefinitions>
             <ColumnDefinition/>
@@ -13,7 +13,7 @@
         </Grid.ColumnDefinitions>
         <WrapPanel>
 
-            <StackPanel Orientation="Horizontal" Margin="2" Visibility="Collapsed">
+            <StackPanel Orientation="Horizontal" Margin="2" >
                 <Button Content="检测通信" Tag="FS_GCS_ENABLE" Click="Modify_Select" Width="130"/>
                 <Label Content="_FS_GCS_ENABLE 0:关闭 1开启"/>
             </StackPanel>
@@ -28,17 +28,17 @@
                 <Label Content="_NTF_LED_BRIGHT 1-3"/>
             </StackPanel>
 
-            <StackPanel Orientation="Horizontal" Margin="2" Visibility="Collapsed">
+            <StackPanel Orientation="Horizontal" Margin="2" >
                 <Button Content="GPS类型灯光" Tag="NTF_G_RTKTEST" Click="Modify_Select" Width="130"/>
                 <Label Content="_NTF_G_RTKTEST 0:关闭 1:开启"/>
             </StackPanel>
 
-            <StackPanel Orientation="Horizontal" Margin="2" Visibility="Collapsed">
+            <StackPanel Orientation="Horizontal" Margin="2" >
                 <Button Content="š‹气压计‹GPS类型" Tag="EK2_ALT_GPS" Click="Modify_Select" Width="130"/>
                 <Label Content="_EK2_ALT_GPS 1-6"/>
             </StackPanel>
 
-            <StackPanel Orientation="Horizontal" Margin="2" Visibility="Collapsed">
+            <StackPanel Orientation="Horizontal" Margin="2" >
                 <Button Content="š最低解锁电压" Tag="ARMING_VOLT_MIN" Click="Modify_Select" Width="130"/>
                 <Label Content="_ARMING_VOLT_MIN"/>
             </StackPanel>
@@ -48,22 +48,22 @@
                 <Label Content="_NTF_G_RTLOFF"/>
             </StackPanel>
 
-            <StackPanel Orientation="Horizontal" Margin="2" Visibility="Collapsed">
+            <StackPanel Orientation="Horizontal" Margin="2" >
                 <Button Content="š航点灯光" Tag="WAYPOINT_GLED" Click="Modify_Select" Width="130"/>
                 <Label Content="_WAYPOINT_GLED"/>
             </StackPanel>
 
-            <StackPanel Orientation="Horizontal" Margin="2" Visibility="Collapsed">
+            <StackPanel Orientation="Horizontal" Margin="2" >
                 <Button Content="š解锁检查" Tag="ARMING_CHECK" Click="Modify_Select" Width="130"/>
                 <Label Content="_ARMING_CHECK 414"/>
             </StackPanel>
 
-            <StackPanel Orientation="Horizontal" Margin="2" Visibility="Collapsed">
+            <StackPanel Orientation="Horizontal" Margin="2" >
                 <Button Content="š解锁GPS类型" Tag="ARMING_GPS_LEVEL" Click="Modify_Select" Width="130"/>
                 <Label Content="_ARMING_GPS_LEVEL 1-6"/>
             </StackPanel>
 
-            <StackPanel Orientation="Horizontal" Margin="2" Visibility="Collapsed">
+            <StackPanel Orientation="Horizontal" Margin="2" >
                 <Button Content="š返航GPS类型" Tag="FS_GPS_RTL" Click="Modify_Select" Width="130"/>
                 <Label Content="_FS_GPS_RTL 1-6"/>
             </StackPanel>
@@ -77,7 +77,7 @@
         <StackPanel Grid.Column="1" HorizontalAlignment="Center" Orientation ="Horizontal">
             <WrapPanel VerticalAlignment="Center" Orientation="Vertical" >
                 <Label x:Name="label" Content="参数名称" Margin="5,0,5,5"/>
-                <TextBox x:Name="textParamName" Width="160" Margin="5" IsReadOnly="True"/>
+                <TextBox x:Name="textParamName" Width="160" Margin="5" IsReadOnly="False"/>
                 <Label x:Name="label_Copy" Content="参数值" Margin="5"/>
                 <TextBox x:Name="textParamValue" Margin="5"/>
                 <Button x:Name="btnModify" Content="修改" Width="100" Margin="10" Click="btnModify_Click"/>

Plane.FormationCreator/Plane.FormationCreator.csproj

@@ -181,6 +181,7 @@
     <Compile Include="Formation\FlightTaskManager.cs" />
     <Compile Include="Formation\PropertyChangedEventArgs.cs" />
     <Compile Include="Formation\FlightTask_TakeOff.cs" />
+    <Compile Include="Formation\View3DManager.cs" />
     <Compile Include="GMap.cs" />
     <Compile Include="Maps\MapConverter.cs" />
     <Compile Include="ModifyParam.xaml.cs">
@@ -190,6 +191,7 @@
     <Compile Include="Test.cs" />
     <Compile Include="CalculationLogLatDistance.cs" />
     <Compile Include="Util\ParamFile.cs" />
+    <Compile Include="ViewModels\CalibrationViewModel.cs" />
     <Compile Include="ViewModels\ConnectViewModel.cs" />
     <Compile Include="ViewModels\ControlPanelViewModel.cs" />
     <Compile Include="ViewModels\CopterListViewModel.cs" />
@@ -199,6 +201,9 @@
     <Compile Include="ViewModels\ModiLEDModel.cs" />
     <Compile Include="ViewModels\TaskBarViewModel.cs" />
     <Compile Include="ViewModels\View3DViewModel.cs" />
+    <Compile Include="Views\CalibrationWindow.xaml.cs">
+      <DependentUpon>CalibrationWindow.xaml</DependentUpon>
+    </Compile>
     <Compile Include="Views\ControlPanelView.xaml.cs">
       <DependentUpon>ControlPanelView.xaml</DependentUpon>
     </Compile>
@@ -311,6 +316,10 @@
       <SubType>Designer</SubType>
       <Generator>MSBuild:Compile</Generator>
     </Page>
+    <Page Include="Views\CalibrationWindow.xaml">
+      <SubType>Designer</SubType>
+      <Generator>MSBuild:Compile</Generator>
+    </Page>
     <Page Include="Views\ControlPanelView.xaml">
       <SubType>Designer</SubType>
       <Generator>MSBuild:Compile</Generator>

Plane.FormationCreator/ServiceLocatorConfigurer.cs

@@ -33,11 +33,13 @@ namespace Plane.FormationCreator
             _container.Register<TaskBarViewModel>();
             _container.Register<View3DViewModel>();
             _container.Register<ModifyTaskViewModel>();
+            _container.Register<CalibrationViewModel>();
 
             _container.Register<ILogger>(() => new LocalFileLogger(new DebugLogger()));
 
             _container.Register<CopterManager>();
             _container.Register<MapManager>();
+            _container.Register<View3DManager>();
             _container.Register<FlightTaskManager>();
 
             _container.Register<FormationController>();

Plane.FormationCreator/ViewModels/ConnectViewModel.cs

@@ -202,6 +202,7 @@ namespace Plane.FormationCreator.ViewModels
             {
                 return _UpdateAllCopterCommand ?? (_UpdateAllCopterCommand = new RelayCommand(async () =>
                 {
+                    Message.Show("-----开始空中升级------");
                     await commModule.UpdateCommModule();
                 }));
             }

Plane.FormationCreator/ViewModels/ControlPanelViewModel.cs

@@ -19,6 +19,7 @@ using Microsoft.Win32;
 using Plane.Util;
 using System.Windows.Media;
 using Plane.CommunicationManagement;
+using Plane.FormationCreator.Views;
 
 namespace Plane.FormationCreator.ViewModels
 {
@@ -294,11 +295,7 @@ namespace Plane.FormationCreator.ViewModels
                 {
                     if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
                     {
-                        for (int i = 1; i <= 4; i++)
+                        await _commModuleManager.MotorTestAsync(_copterManager.AcceptingControlCopters);
-                        {
-                            await _commModuleManager.MotorTestAsync(i, _copterManager.AcceptingControlCopters);
-                            await Task.Delay(10);
-                        }
                     }
                     
                     /*
@@ -325,7 +322,7 @@ namespace Plane.FormationCreator.ViewModels
                 return _GuidAsyncCommand ?? (_GuidAsyncCommand = new RelayCommand(async () =>
                 {
                     if (_copterManager.AcceptingControlCopters != null && _copterManager.AcceptingControlCopters.Count() > 0)
-                        await _commModuleManager.TakeOffAsync(1);
+                        await _commModuleManager.TakeOffAsync(1, _copterManager.AcceptingControlCopters);
                     /*
                     await Task.WhenAll(_copterManager.AcceptingControlCopters.Select(async c =>
                     {
@@ -518,6 +515,22 @@ namespace Plane.FormationCreator.ViewModels
                 }));
             }
         }
+
+        //打开校准界面
+        private ICommand _CalibrationSingleCommand;
+        public ICommand CalibrationSingleCommand
+        {
+            get
+            {
+                return _CalibrationSingleCommand ?? (_CalibrationSingleCommand = new RelayCommand(async () =>
+                {
+                    CalibrationWindow calibrationWindow = new CalibrationWindow();
+                    calibrationWindow.ShowDialog();
+                    await Task.Delay(100);
+                }));
+            }
+        }
+
         private ICommand _LoadParamfile;
         public ICommand LoadParamfile
         {

Plane.FormationCreator/ViewModels/CopterListViewModel.cs

@@ -15,6 +15,7 @@ using System.Windows.Input;
 using System.Threading;
 using Plane.Communication;
 using Plane.Geography;
+using Microsoft.Practices.ServiceLocation;
 
 namespace Plane.FormationCreator.ViewModels
 {
@@ -56,6 +57,7 @@ namespace Plane.FormationCreator.ViewModels
         private CopterManager _copterManager;
         private MapManager _mapManager;
         private FlightTaskManager _flightTaskManager;
+        private View3DManager _view3DManager = ServiceLocator.Current.GetInstance<View3DManager>();
 
         public CopterManager CopterManager { get { return _copterManager; } }
 
@@ -128,10 +130,11 @@ namespace Plane.FormationCreator.ViewModels
             {
                 return _AddVirtualCopterCommand ?? (_AddVirtualCopterCommand = new RelayCommand<int>(async addcount =>
                 {
-                    if (_copterManager.Copters.Count() >= 50)
+                    //给第三方时候限制数量用
-                    {
+//                     if (_copterManager.Copters.Count() >= 50)
-                        return;
+//                     {
-                    }
+//                         return;
+//                     }
 
                     var center = _mapManager.Center;
                     string id;
@@ -139,6 +142,11 @@ namespace Plane.FormationCreator.ViewModels
                     int colnum = _flightTaskManager.ColumnCount; //自动生成列数=4
                     float coldis = _flightTaskManager.ColumnDistance;//列相距5米
                     float rowdis = _flightTaskManager.RowDistance;//行相距5米
+                    int orientation = _flightTaskManager.Orientation; //矩阵朝向
+
+                    
+
+
                     int currcol = 0; //当前列号
                     int currrow = 0; //当前行
                     Tuple<double, double> colheadLatLng = new Tuple<double, double>(0, 0);
@@ -162,12 +170,12 @@ namespace Plane.FormationCreator.ViewModels
                         else
                         {
                             if (currcol < colnum)
-                                targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Value.Lat, colLatLng.Value.Lng , 90, currcol * coldis);
+                                targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Value.Lat, colLatLng.Value.Lng , orientation + 90, currcol * coldis);
                             else
                             {
                                 currrow++;
                                 currcol = 0;
-                                targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Value.Lat, colLatLng.Value.Lng, 180, rowdis);
+                                targetLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(colLatLng.Value.Lat, colLatLng.Value.Lng, orientation + 180, rowdis);
                                 colLatLng = new LatLng(targetLatLng.Item1, targetLatLng.Item2); 
 
                                 colheadLatLng = targetLatLng;
@@ -245,6 +253,7 @@ namespace Plane.FormationCreator.ViewModels
                     _copterManager.Copters.Clear();
                     _copterManager.CopterStatus.Clear();
                     _mapManager.ClearCopters();
+                    _view3DManager.ClearCopters();
                     _flightTaskManager.ClearTasks();
                     //UdpServerConnectionManager.Instance.ClearConnections();
                     _virtualCopterId = 1;

Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs

@@ -14,6 +14,7 @@ using System.Windows.Input;
 using Plane.Geography;
 using Plane.Windows.Messages;
 using System.Windows;
+using System.Windows.Media;
 
 namespace Plane.FormationCreator.ViewModels
 {
@@ -698,25 +699,23 @@ public ICommand VerticlAlignmentCommand
         }
 
 
+        public static double CalculateFlyIime(double s, double v)
+        {
+            double t;
+            double a = 1;  //加速度1米每秒
+            double at = v / a;
+            double a_s = 0.5f * a * at * at;
+            if (a_s > (s / 2)) //还没到特定速度就到了中点了
+            {
+                t = (float)System.Math.Sqrt(s / a) * 2;
+            }
+            else
+            {
+                t = (s - a_s * 2) / v + at * 2;
+            }
 
-
+            return t;
-
+        }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
 
 
 
@@ -752,6 +751,8 @@ public ICommand VerticlAlignmentCommand
             WGS84,
         }
 
+       
+
 
         //水平旋转
         private ICommand _LevelRotateCommand;
@@ -923,6 +924,8 @@ public ICommand VerticlAlignmentCommand
                                     );
 
                             }
+
+                            _flightTaskManager.Orientation += RotateLine;
                         }
 
 
@@ -1267,30 +1270,43 @@ public ICommand VerticlAlignmentCommand
             }
         }
 
-        private ICommand _WayPointDistinceCommand;
+
-        public ICommand WayPointDistinceCommand
+        private ICommand _MaxDistinceAndTimeCommand;
+        public ICommand MaxDistinceAndTimeCommand
         {
             get
             {
-                return _WayPointDistinceCommand ?? (_WayPointDistinceCommand = new RelayCommand<double>(async =>
+                return _MaxDistinceAndTimeCommand ?? (_MaxDistinceAndTimeCommand = new RelayCommand<double>(async =>
                 {
-                    if (_copterManager.AcceptingControlCopters.Count() != 1)
-                        return;
-
                     int taskIndex = _flightTaskManager.SelectedTaskIndex;
                     if (taskIndex != 0 || _flightTaskManager.SelectedTask.TaskType == FlightTaskType.FlyTo)
                     {
-                        var curCopter = _copterManager.AcceptingControlCopters.First();
+                        if (_copterManager.AcceptingControlCopters.Count() > 0)
+                        {
+                            double maxDistance = 0.0f;
+                            string copterName = "";
+                            double speed = 0.0f;
+                            foreach (var copter in _copterManager.AcceptingControlCopters)
+                            {
+                                var prevWaypoint = _flightTaskManager.Tasks[taskIndex - 1].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
+                                var curWaypoint = _flightTaskManager.Tasks[taskIndex].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
 
-                        var prevWaypoint = _flightTaskManager.Tasks[taskIndex - 1].SingleCopterInfos.FirstOrDefault(c => c.Copter == curCopter);
+                                double distance = GeographyUtils.CalcDistance(
-                        var curWaypoint = _flightTaskManager.Tasks[taskIndex].SingleCopterInfos.FirstOrDefault(c => c.Copter == curCopter);
+                                       prevWaypoint.TargetLat, prevWaypoint.TargetLng, prevWaypoint.TargetAlt,
+                                       curWaypoint.TargetLat, curWaypoint.TargetLng, curWaypoint.TargetAlt);
+                                if (distance > maxDistance)
+                                {
+                                    maxDistance = distance;
+                                    copterName = copter.Name;
+                                    speed = curWaypoint.LevelSpeed;
+                                }
+                            }
 
-                        double distance = GeographyUtils.CalcDistance(
+                            double time = CalculateFlyIime(maxDistance, speed);
-                               prevWaypoint.TargetLat, prevWaypoint.TargetLng, prevWaypoint.TargetAlt,
+                            Message.Show($"最大航点间距 = {Math.Round(maxDistance, 2)}米, 水平速度={Math.Round(speed, 2)}, 飞行时间 = {Math.Round(time, 2)}秒, 飞机编号:{copterName}");
-                               curWaypoint.TargetLat, curWaypoint.TargetLng, curWaypoint.TargetAlt);
+                        }
-
-                        Message.Show($"航点间距 = {distance}");
                     }
+
                 }));
             }
         }
@@ -1298,6 +1314,7 @@ public ICommand VerticlAlignmentCommand
 
 
 
+
         //缩放
         private ICommand _ScaleCommand;
         public ICommand ScaleCommand
@@ -1380,6 +1397,7 @@ public ICommand VerticlAlignmentCommand
             }
         }
 
+
         private ICommand _BackToPreviousTaskPoint;
         public ICommand BackToPreviousTaskPoint
         {
@@ -1391,5 +1409,252 @@ public ICommand VerticlAlignmentCommand
                 }));
             }
         }
+
+        #region 跑马灯参数
+
+        private double _BeginTime = 10;
+        public double BeginTime
+        {
+            get { return _BeginTime; }
+            set { Set(nameof(BeginTime), ref _BeginTime, value); }
+        }
+
+        private double _EndTime = 11;
+        public double EndTime
+        {
+            get { return _EndTime; }
+            set { Set(nameof(EndTime), ref _EndTime, value); }
+        }
+
+        private double _AverageSum = 10;
+        public double AverageSum
+        {
+            get { return _AverageSum; }
+            set { Set(nameof(AverageSum), ref _AverageSum, value); }
+        }
+
+        private string _ChangeRGB = "FFFFFF";
+        public string ChangeRGB
+        {
+            get { return _ChangeRGB; }
+            set { Set(nameof(ChangeRGB), ref _ChangeRGB, value); }
+        }
+
+        private string _EndRGB = "";
+        public string EndRGB
+        {
+            get { return _EndRGB; }
+            set { Set(nameof(EndRGB), ref _EndRGB, value); }
+        }
+        #endregion
+
+        //跑马灯--按照经纬度变换灯光
+        private ICommand _SetHorseRaceLampCommand;
+        public ICommand SetHorseRaceLampCommand
+        {
+            get
+            {
+                return _SetHorseRaceLampCommand ?? (_SetHorseRaceLampCommand = new RelayCommand(async () =>
+                {
+
+                    if (_copterManager.SelectedCopters.Count() <= 0)
+                        return;
+
+                    double minLat, maxLat;
+                    double minLng, maxLng;
+
+
+
+                    //所以选中飞行当前任务信息
+                    var selectCopterInfos = _flightTaskManager.SelectedTask.SingleCopterInfos.FindAll(delegate (FlightTaskSingleCopterInfo p)
+                    {
+                        return _copterManager.SelectedCopters.Contains(p.Copter);
+                    });
+
+                    minLat = selectCopterInfos.Min(c => c.TargetLat);
+                    maxLat = selectCopterInfos.Max(c => c.TargetLat);
+                    minLng = selectCopterInfos.Min(c => c.TargetLng);
+                    maxLng = selectCopterInfos.Max(c => c.TargetLng);
+
+                    double averagelngCount = maxLng - minLng;
+
+                    foreach (var copterInfo in selectCopterInfos)
+                    {
+                        double interval = copterInfo.TargetLng - minLng;
+                        int time = (int)(interval / (averagelngCount / AverageSum));
+                        LEDInfo led = new LEDInfo();
+                        led.LEDMode = 0;
+                        led.Delay = BeginTime + time * (EndTime - BeginTime) / AverageSum;
+                        led.LEDRGB = ChangeRGB;
+                        copterInfo.AddLEDInfo(led);
+
+                        //跑马灯变化后改变颜色 效果类似灯带
+                        if (EndRGB != "")
+                        {
+                            LEDInfo endLed = new LEDInfo();
+                            endLed.LEDMode = 0;
+                            endLed.Delay = BeginTime + (time + 1) * (EndTime - BeginTime) / AverageSum;
+                            endLed.LEDRGB = EndRGB;
+                            copterInfo.AddLEDInfo(endLed);
+                        }
+                    }
+
+                    await Task.Delay(100);
+                }));
+            }
+        }
+
+
+        private double _StrokesTime = 10;
+        public double StrokesTime
+        {
+            get { return _StrokesTime; }
+            set { Set(nameof(StrokesTime), ref _StrokesTime, value); }
+        }
+
+        private double _IntervalTime = 0.1;
+        public double IntervalTime
+        {
+            get { return _IntervalTime; }
+            set { Set(nameof(IntervalTime), ref _IntervalTime, value); }
+        }
+
+        private double _SingleNum = 1;
+        public double SingleNum
+        {
+            get { return _SingleNum; }
+            set { Set(nameof(SingleNum), ref _SingleNum, value); }
+        }
+
+        private string _StrokesRGB = "FFFFFF";
+        public string StrokesRGB
+        {
+            get { return _StrokesRGB; }
+            set { Set(nameof(StrokesRGB), ref _StrokesRGB, value); }
+        }
+
+        //跑马灯(笔画顺序用)--按照选择航点顺序变换灯光
+        private ICommand _SetStrokesLampCommamd;
+        public ICommand SetStrokesLampCommamd
+        {
+            get
+            {
+                return _SetStrokesLampCommamd ?? (_SetStrokesLampCommamd = new RelayCommand(async () =>
+                {
+                    if (_copterManager.SelectedCopters.Count() <= 0)
+                        return;
+
+
+                    double curTime = StrokesTime;
+                    int curcount = 1;
+                    foreach (var copter in _copterManager.SelectedCopters)
+                    {
+                        if (curcount > SingleNum)
+                        {
+                            curTime += IntervalTime;
+                            curcount = 1;
+                        }
+                            
+
+                        var copterInfo = _flightTaskManager.SelectedTask.SingleCopterInfos.FirstOrDefault(c=>c.Copter == copter);
+
+                        LEDInfo led = new LEDInfo();
+                        led.LEDMode = 0;
+                        led.Delay = curTime;
+                        led.LEDRGB = StrokesRGB;
+                        copterInfo.AddLEDInfo(led);
+
+                        curcount++;
+                    }
+
+                    await Task.Delay(100);
+                }));
+            }
+        }
+
+
+
+        #region 渐变灯参数
+        private double _GradientRampTime = 10;
+        public double GradientRampTime
+        {
+            get { return _GradientRampTime; }
+            set { Set(nameof(GradientRampTime), ref _GradientRampTime, value); }
+        }
+
+        private string _RightRGB = "FFFFFF";
+        public string RightRGB
+        {   
+            get { return _RightRGB; }
+            set { Set(nameof(RightRGB), ref _RightRGB, value); }
+        }
+
+        private string _LeftRGB = "FFFFFF";
+        public string LeftRGB
+        {
+            get { return _LeftRGB; }
+            set { Set(nameof(LeftRGB), ref _LeftRGB, value); }
+        }
+        #endregion
+
+        //渐变色
+        private ICommand _SetGradientRampCommand;
+        public ICommand SetGradientRampCommand
+        {
+            get
+            {
+                return _SetGradientRampCommand ?? (_SetGradientRampCommand = new RelayCommand(async () =>
+                {
+
+                    if (_copterManager.SelectedCopters.Count() <= 0)
+                        return;
+
+                    double minLat, maxLat;
+                    double minLng, maxLng;
+
+
+                    //所以选中飞行当前任务信息
+                    var selectCopterInfos = _flightTaskManager.SelectedTask.SingleCopterInfos.FindAll(delegate (FlightTaskSingleCopterInfo p)
+                    {
+                        return _copterManager.SelectedCopters.Contains(p.Copter);
+                    });
+
+                    minLat = selectCopterInfos.Min(c => c.TargetLat);
+                    maxLat = selectCopterInfos.Max(c => c.TargetLat);
+                    minLng = selectCopterInfos.Min(c => c.TargetLng);
+                    maxLng = selectCopterInfos.Max(c => c.TargetLng);
+
+                    double averagelngCount = maxLng - minLng;
+                   
+                    Color rightColor = (Color)ColorConverter.ConvertFromString("#" + RightRGB);
+                    Color leftColor = (Color)ColorConverter.ConvertFromString("#" + LeftRGB);
+
+                    int diffR = rightColor.R - leftColor.R;
+                    int diffG = rightColor.G - leftColor.G;
+                    int diffB = rightColor.B - leftColor.B;
+
+                    foreach (var copterInfo in selectCopterInfos)
+                    {
+                        double interval = copterInfo.TargetLng - minLng;
+                        double percent = (interval / averagelngCount );
+
+                        int R = (int)(diffR * percent) + leftColor.R;
+                        int G = (int)(diffG * percent) + leftColor.G;
+                        int B = (int)(diffB * percent) + leftColor.B;
+                        Color c = Color.FromRgb((byte)R, (byte)G, (byte)B);
+
+                        LEDInfo led = new LEDInfo();
+                        led.LEDMode = 0;
+                        led.Delay = GradientRampTime;
+
+                        //led.LEDRGB = Convert.ToString(R, 16) + Convert.ToString(G, 16) + Convert.ToString(B, 16);
+                        led.LEDRGB = R.ToString("X2") + G.ToString("X2") + B.ToString("X2");
+                        copterInfo.AddLEDInfo(led);
+                    }
+
+                    await Task.Delay(100);
+                }));
+            }
+        }
     }
 }

Plane.FormationCreator/ViewModels/View3DViewModel.cs

@@ -1,10 +1,15 @@
 using GalaSoft.MvvmLight;
 using GalaSoft.MvvmLight.Command;
 using HelixToolkit.Wpf;
+using Microsoft.Practices.ServiceLocation;
+using Plane.Collections;
+using Plane.Copters;
 using Plane.FormationCreator.Formation;
 using Plane.Geography;
+using Plane.Windows.Messages;
 using System;
 using System.Collections.Generic;
+using System.Collections.Specialized;
 using System.ComponentModel;
 using System.Linq;
 using System.Text;
@@ -18,17 +23,25 @@ namespace Plane.FormationCreator.ViewModels
     public class View3DViewModel : ViewModelBase
     {
         private FlightTaskManager _flightTaskManager;
+        private CopterManager _copterManager;
 
-        public View3DViewModel(FlightTaskManager flightTaskManager)
+        private View3DManager _view3DManager = ServiceLocator.Current.GetInstance<View3DManager>();
+
+        public View3DViewModel(FlightTaskManager flightTaskManager, CopterManager copterManager)
         {
             modelGroup = new Model3DGroup();
             planeGroup = new Model3DGroup();
             waypointGroup = new Model3DGroup();
-            modelGroup.Children.Add(planeGroup);
             modelGroup.Children.Add(waypointGroup);
+            modelGroup.Children.Add(planeGroup);
+
             this.Model = modelGroup;
             _flightTaskManager = flightTaskManager;
+            _copterManager = copterManager;
             _flightTaskManager.PropertyChanged += new PropertyChangedEventHandler(flightTaskPropertyChanged);
+            _view3DManager._view3DViewModel = this;
+
+            _copterManager.Copters.CollectionChanged += Copters_CollectionChanged;
         }
 
         private void flightTaskPropertyChanged(object sender, PropertyChangedEventArgs e)
@@ -43,32 +56,174 @@ namespace Plane.FormationCreator.ViewModels
             }
         }
 
+        private void Copters_CollectionChanged(object sender, NotifyCollectionChangedEventArgs e)
+        {
+            e.OldItems?.ForEach<ICopter>(async copter =>
+            {
+                copter.LocationChanged -= Copter_LocationChanged;
+                await Task.Delay(100);  // 如果不等待一段时间，Copter_DataStreamReceived 很可能再被调用一次。
+                Remove3DCopter(copter);
+            });
+            e.NewItems?.ForEach<ICopter>(copter => copter.LocationChanged += Copter_LocationChanged);
+        }
+
+        private void Remove3DCopter(ICopter copter)
+        {
+            if (planeModel3Ds.ContainsKey(copter))
+            {
+                GeometryModel3D panle3D = planeModel3Ds[copter];
+                planeGroup.Children.Remove(panle3D);
+                planeModel3Ds.Remove(copter);
+                panle3D = null;
+            }
+        }
+
+        public void Clear3DCopters()
+        {
+            planeModel3Ds.Clear();
+            planeGroup.Children.Clear();
+            observationLatLng = null;
+        }
+
+        private void Copter_LocationChanged(object sender, EventArgs e)
+        {
+            var copter = sender as ICopter;
+            if (App.Current.CheckAccess())
+            {
+                AddOrMove3DCopter(copter);
+            }
+            else
+            {
+                App.Current.Dispatcher.InvokeAsync(() => AddOrMove3DCopter(copter));
+            }
+        }
+
+        private Dictionary<ICopter, GeometryModel3D> planeModel3Ds = new Dictionary<ICopter, GeometryModel3D>();
+        Tuple<double, double> observationLatLng = null;
+
+        private void AddOrMove3DCopter(ICopter copter)
+        {
+            var copternum1 = _copterManager.Copters.FirstOrDefault();
+            //第一列到中间的距离
+            float midColDistance = (_flightTaskManager.ColumnCount - 1) * _flightTaskManager.ColumnDistance / 2;
+
+            if (observationLatLng == null)
+            {
+                observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
+                   copternum1.Latitude,
+                   copternum1.Longitude,
+                   _flightTaskManager.Orientation + 90,
+                   midColDistance);
+                observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
+                   observationLatLng.Item1,
+                   observationLatLng.Item2,
+                   _flightTaskManager.Orientation + 180,
+                   midColDistance * 2);
+            }
+               
+            if (!planeModel3Ds.ContainsKey(copter))
+            {
+                
+                //观测点的位置放到最后排飞机的中间位置
+                
+                var meshBuilderwaypoint = new MeshBuilder(false, false);
+
+                //meshBuilderwaypoint.AddTriangle(new Point3D(-0.1, -0.1, 0), new Point3D(0.1, -0.1, 0), new Point3D(0, 0.2, 0));
+                meshBuilderwaypoint.AddSphere(new Point3D(0, 0, 0), 0.2);
+                var meshwaypoint = meshBuilderwaypoint.ToMesh(true);
+
+                double y = GeographyUtils.CalcDistance(0, observationLatLng.Item1, 0, 0, copter.Latitude, 0) / 2;
+                if (observationLatLng.Item1 > copter.Latitude) y = -y;
+
+                double x = GeographyUtils.CalcDistance(observationLatLng.Item2, 0, 0, copter.Longitude, 0, 0) / 2;
+                if (observationLatLng.Item2 > copter.Longitude) x = -x;
+
+
+
+                var model3D = new GeometryModel3D
+                {
+                    Geometry = meshwaypoint,
+                    Transform = new TranslateTransform3D(x, y, copter.Altitude + 0.1),
+                    Material = MaterialHelper.CreateMaterial(Color.FromRgb(238, 210, 238)),
+                    BackMaterial = MaterialHelper.CreateMaterial(Color.FromRgb(238, 210, 238))
+                };
+
+                planeModel3Ds.Add(copter, model3D);
+                planeGroup.Children.Add(model3D);
+            }
+            else
+            {
+                
+                double y = GeographyUtils.CalcDistance(0, observationLatLng.Item1, 0, 0, copter.Latitude, 0) / 2;
+                if (observationLatLng.Item1 > copter.Latitude) y = -y;
+
+                double x = GeographyUtils.CalcDistance(observationLatLng.Item2, 0, 0, copter.Longitude, 0, 0) / 2;
+                if (observationLatLng.Item2 > copter.Longitude) x = -x;
+
+                //保留小数后2位，精确到1cm
+                x = Math.Round(x, 2);//按照四舍五入的国际标准
+                y = Math.Round(y, 2);
+                double z = Math.Round(copter.Altitude / 2, 2);
+
+                GeometryModel3D panle3D = planeModel3Ds[copter];
+
+                if (x != panle3D.Transform.Value.OffsetX || y != panle3D.Transform.Value.OffsetY || z != panle3D.Transform.Value.OffsetZ)
+                {
+                    panle3D.Transform = new TranslateTransform3D(x, y, z);
+                }
+
+                if (copter.LEDColor != null && copter.LEDColor != "")
+                {
+                    Color color = (Color)ColorConverter.ConvertFromString("#" + copter.LEDColor);
+                    panle3D.Material = MaterialHelper.CreateMaterial(color);
+                    panle3D.BackMaterial = MaterialHelper.CreateMaterial(color);
+                }
+                   
+            }
+            
+            //Message.Show("添加3D飞机：" + copter.Name);
+        }
+
+
+
         private void SelectTask()
         {
+            if (waypointGroup == null) waypointGroup = new Model3DGroup();
+            waypointGroup.Children.Clear();
             if (_flightTaskManager.SelectedTaskIndex > 0)
             {
+                //观测点的位置放到最后排飞机的中间位置
+                Tuple<double, double> observationLatLng;
+
+                //列的中间位置
+                float midDistance =  (_flightTaskManager.ColumnCount - 1) * _flightTaskManager.ColumnDistance / 2;
+
+                observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
+                    _flightTaskManager.OriginLat, _flightTaskManager.OriginLng, _flightTaskManager.Orientation + 90, midDistance);
+                observationLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(
+                    observationLatLng.Item1, observationLatLng.Item2, _flightTaskManager.Orientation + 180, midDistance * 2);
+
                 //modelGroup.Children.Clear();
-                if (waypointGroup == null) waypointGroup = new Model3DGroup();
-                waypointGroup.Children.Clear();
-               
 
                 var meshBuilderwaypoint = new MeshBuilder(false, false);
-                meshBuilderwaypoint.AddSphere(new Point3D(0, 0, 0), 0.3);
+                meshBuilderwaypoint.AddSphere(new Point3D(0, 0, 0), 0.2);
                 var meshwaypoint = meshBuilderwaypoint.ToMesh(true);
                 var greenMaterial = MaterialHelper.CreateMaterial(Color.FromRgb(0,255,0));
 
                 foreach (FlightTaskSingleCopterInfo info in _flightTaskManager.Tasks[_flightTaskManager.SelectedTaskIndex].SingleCopterInfos)
                 {
-                
+
-                    double x = GeographyUtils.CalcDistance(_flightTaskManager.OriginLng, 0, 0, info.TargetLng, 0, 0);
+                    double y = GeographyUtils.CalcDistance(0, observationLatLng.Item1, 0, 0, info.TargetLat, 0)/2;
-                    if (_flightTaskManager.OriginLng > info.TargetLng) x = -x;
+                    if (observationLatLng.Item1 > info.TargetLat) y = -y;
-                    double y = GeographyUtils.CalcDistance(0, _flightTaskManager.OriginLat, 0, 0, info.TargetLat, 0);
+
-                    if (_flightTaskManager.OriginLat > info.TargetLat) y = -y;
+                    double x = GeographyUtils.CalcDistance(observationLatLng.Item2, 0, 0, info.TargetLng, 0, 0)/2;
+                    if (observationLatLng.Item2 > info.TargetLng) x = -x;
+
 
                     waypointGroup.Children.Add(new GeometryModel3D
                     {
                         Geometry = meshwaypoint,
-                        Transform = new TranslateTransform3D(x, y, info.TargetAlt),
+                        Transform = new TranslateTransform3D(x, y, info.TargetAlt/2),
                         Material = greenMaterial,
                         BackMaterial = greenMaterial
                     });

Plane.FormationCreator/Views/ConnectWindow.xaml

@@ -85,7 +85,7 @@
                  Visibility="Collapsed"
                  Text="{Binding IPs, UpdateSourceTrigger=PropertyChanged}" />
 
-        <StackPanel Orientation="Horizontal" Visibility="Collapsed"
+        <StackPanel Orientation="Horizontal"
                     HorizontalAlignment="Center"
                     Grid.Row="1"
                     Grid.ColumnSpan="3">
@@ -125,7 +125,7 @@
                                CommandParameter="UDP" />-->
             <Button Content="关闭TCP" Margin="5" Command="{Binding CloseCommand}"/>
         </StackPanel>
-        <StackPanel Orientation="Horizontal" Visibility="Collapsed"
+        <StackPanel Orientation="Horizontal"
                     HorizontalAlignment="Center"
                     Grid.Row="2"
                     Grid.ColumnSpan="3">
@@ -140,12 +140,12 @@
                     Grid.Row="3"
                     Grid.ColumnSpan="3">
             <TextBox Margin="2,5,5,5" Width="30" Text="{Binding CopterNum}"></TextBox>
-            <Button Content="对频"  Margin="5,5,0,5" Command="{Binding Path=WriteIdCommand}" />
+            <Button Content="    对频    "  Margin="5,5,0,5" Command="{Binding Path=WriteIdCommand}" />
-            <Button Content="闪灯" Visibility="Collapsed" Margin="5" Command="{Binding CommDataAsync}"/>
+            <Button Content="闪灯"  Margin="5" Command="{Binding CommDataAsync}"/>
-            <Button Content="搜索飞机" Visibility="Collapsed" Margin="5" Command="{Binding QueryAllCopterCommand}"/>
+            <Button Content="搜索飞机"  Margin="5" Command="{Binding QueryAllCopterCommand}"/>
         </StackPanel>
 
-        <StackPanel Orientation="Horizontal" Visibility="Collapsed"
+        <StackPanel Orientation="Horizontal" 
                     HorizontalAlignment="Center"
                     Grid.Row="4"
                     Grid.ColumnSpan="3">

Plane.FormationCreator/Views/ControlPanelView.xaml

@@ -40,14 +40,16 @@
                      Command="{Binding GuidAsyncCommand}" />
             <Button Content="悬停"
                     Command="{Binding HoverCommand}" />
-            <Button Content="手动" Visibility="Collapsed"
+            <Button Content="手动" 
                     Command="{Binding FloatCommand}" />
             <Button Content="参数"
                     Command="{Binding ParamModify}" />
             <Button Content="参数文件" Visibility="Collapsed"
                     Command="{Binding LoadParamfile}" />
-            <Button Content="版本" Visibility="Collapsed"
+            <Button Content="版本"
                     Command="{Binding GetVersionsCommand}" />
+            <Button Content="校准"
+                    Command="{Binding CalibrationSingleCommand}" />
         </WrapPanel>
         <WrapPanel>
             <Button Content="返航"
@@ -62,14 +64,14 @@
                     Command="{Binding LEDFlickerCommand}" />
             <Button Content="测试" Visibility="Collapsed"
                     Command="{Binding TestCommand}" />
-            <Button Content="开灯" Visibility="Collapsed"
+            <Button Content="开灯" 
                     Command="{Binding LEDOnOffCommand}" 
                     CommandParameter="0"/>
-            <Button Content="关灯" Visibility="Collapsed"
+            <Button Content="关灯" 
                     Command="{Binding LEDOnOffCommand}" 
                     CommandParameter="1"/>
             <Button Content="电机"
-                    Visibility="Collapsed"
+                    
                      Command="{Binding MotorTestCommand}" />
             
             <TextBox Width="50" 
@@ -77,9 +79,10 @@
                      Text="{Binding AltP, UpdateSourceTrigger=PropertyChanged}" />
         </WrapPanel>
         <WrapPanel>
+           
             <Button Content="选写航点"
                      Command="{Binding WriteMissionSingleCommand}" />
-            <Button Content="统计模块" Visibility="Collapsed"
+            <Button Content="统计模块" 
                      Command="{Binding DetectionCommModuleVersion}" />
         </WrapPanel>
         <Separator/>

Plane.FormationCreator/Views/MapView.xaml.cs

@@ -215,7 +215,8 @@ namespace Plane.FormationCreator.Views
                 if (rectangleMarker != null && gmap.Markers.Contains(rectangleMarker))
                 {
                     gmap.Markers.Remove(rectangleMarker);
-                    _copterManager.Select(null);
+                    if (!_copterManager.shiftkeydown)
+                        _copterManager.Select(null);
                     if (_flightTaskManager.SelectedTask != null && _flightTaskManager.SelectedTask.TaskType != FlightTaskType.TakeOff)
                     {
                         _copterManager.shiftkeydown = true;
@@ -233,6 +234,7 @@ namespace Plane.FormationCreator.Views
                         }
                         _copterManager.shiftkeydown = false;
                     }
+                   
                 }
                 rectangle = null;
                 rectangleMarker = null;

Plane.FormationCreator/Views/MapView_CopterDrawing.cs

@@ -40,6 +40,8 @@ namespace Plane.FormationCreator.Views
                     _brush = new SolidColorBrush(_color);
                 }
 
+                _brush = new SolidColorBrush(Color.FromRgb(45,45,45));
+
                 _flightTaskManager.TasksCleared += (sender, e) =>
                 {
                     Route.Points.Clear();
@@ -102,7 +104,7 @@ namespace Plane.FormationCreator.Views
 
             GoogleMap.GMap _map;
             Color _color;
-            Brush _brush;
+            public Brush _brush;
 
             ILogger _logger = ServiceLocator.Current.GetInstance<ILogger>();
             CopterManager _copterManager = ServiceLocator.Current.GetInstance<CopterManager>();
@@ -139,7 +141,19 @@ namespace Plane.FormationCreator.Views
                             this.Dot.Fill = new SolidColorBrush(Color.FromRgb(0, 0, 0));
                         else
                             this.Dot.Fill = _brush;
+
+                        if (Copter.LEDColor != null && Copter.LEDColor!="")
+                        {
+                            Color color = (Color)ColorConverter.ConvertFromString("#" + Copter.LEDColor);
+                            _brush = new SolidColorBrush(color);
+                            if (this.Dot.Fill != _brush)
+                            {
+                                this.Dot.Fill = _brush;
+                            }
+                        }
+                        
                     }
+                    
 
                     PointLatLng gmapLatLng = new LatLng(location.Latitude, location.Longitude).ToGCJ02();
                     GPoint center = _map.FromLatLngToLocal(gmapLatLng);
@@ -259,6 +273,14 @@ namespace Plane.FormationCreator.Views
                 }
             }
 
+            public void ChangeBrush(string RGB)
+            {
+                //Color color = (Color)ColorConverter.ConvertFromString("#" + RGB);
+               
+                //_brush = new SolidColorBrush(Color.FromRgb(255, 0, 255));
+                //this.Dot.Fill = _brush;
+            }
+
             public void AddWaypoint(Location location, FlightTaskType type)
             {
                 // Add waypoint.
@@ -593,7 +615,7 @@ namespace Plane.FormationCreator.Views
                 mark.Height = 6;
                 mark.HorizontalAlignment = HorizontalAlignment.Left;
                 mark.VerticalAlignment = VerticalAlignment.Top;
-                mark.Fill = new SolidColorBrush(Colors.Black);
+                mark.Fill = new SolidColorBrush(Colors.GreenYellow);
                 this.Children.Add(mark);
 
                 mark.Visibility = Visibility.Hidden;

Plane.FormationCreator/Views/ModifyTaskView.xaml

@@ -6,9 +6,8 @@
              xmlns:local="clr-namespace:Plane.FormationCreator.Views"
              xmlns:vm="clr-namespace:Plane.FormationCreator.ViewModels"
              xmlns:m="clr-namespace:Plane.FormationCreator.Formation"
-             mc:Ignorable="d"
+             mc:Ignorable="d" Height="420" Width="Auto" MinWidth="350">
-             d:DesignWidth="300" Height="420">
+    <TabControl Margin="0,0,0,0">
-    <TabControl Margin="0,0,-120,-11">
 
         <TabItem Header="航点设计">
             <TabControl SelectedIndex="{Binding FlightTaskManager.SelectedTask.TaskTypeIndex,UpdateSourceTrigger=PropertyChanged}">
@@ -29,7 +28,7 @@
                         </StackPanel.Resources>
                         <TextBlock Margin="5" Text="起飞任务"/>
                         <Separator/>
-                        <StackPanel>
+                        <StackPanel Visibility="Collapsed">
 
                             <TextBlock Text="起飞数量：" Margin="5,10,5,0" />
                             <TextBox x:Name="txttakeoff"
@@ -55,7 +54,7 @@
                             <Button Content="应用所选" Margin="5" Grid.Column="3"
                                     Command="{Binding SetSelTakeOffWaitCommand}"/>
                         </Grid>
-                        
+
                     </StackPanel>
                 </TabItem>
                 <TabItem Header="航点">
@@ -70,7 +69,7 @@
                             <Button Content="导入航点" Command="{Binding ImportWayPointCommand}" />
                             <Button Content="优化路线" Command="{Binding OptimizeRouteCommand}" />
                         </StackPanel>
-                        
+
                         <Separator Margin="0,1"/>
                         <StackPanel>
                             <Button Content="上边对齐"
@@ -132,10 +131,9 @@
                             <Button Content="最小距离"
                                     Margin="0,5,5,0" 
                                     Command="{Binding calDistinceCommand}"/>
-                            <Button Content="航点间距"
+                            <Button Content="最长航距"
-                                    Margin="0,5,5,0" 
+                                    Margin="0,5,5,0"
-                                    Command="{Binding WayPointDistinceCommand}"/>
+                                    Command="{Binding MaxDistinceAndTimeCommand}"/>
-
                             <Button Content="前一高度"
                                     Margin="0,5,5,0"
                                     Command="{Binding PrealtCommand}" />
@@ -161,6 +159,7 @@
                                      HorizontalContentAlignment="Right"
                                      Text="{Binding directionvalue, UpdateSourceTrigger=PropertyChanged}"/>
                             <TextBlock Text="度" Margin="0, 10, 5, 0"/>
+
                         </StackPanel>
 
                         <Separator Margin="0,2" />
@@ -221,7 +220,7 @@
                             <CheckBox Content="返航点" ToolTip="勾选后航点仅高度有用，经纬度无效，无视原点自动飞回起飞点" Margin="0,2,0,0" IsChecked="{Binding IsLandWaypoint,UpdateSourceTrigger=PropertyChanged}" />
                             <Button Content="应用到所选" Margin="0,2,0,0"  Command="{Binding SetIsLandCommand}"  Grid.Column="1"/>
 
-                           
+
                         </Grid>
                         <Separator/>
                         <Grid Margin="0,2"
@@ -230,7 +229,7 @@
                             <Grid.ColumnDefinitions>
                                 <ColumnDefinition/>
                                 <ColumnDefinition/>
-  
+
                             </Grid.ColumnDefinitions>
                             <Grid.RowDefinitions>
                                 <RowDefinition/>
@@ -250,7 +249,7 @@
                                          Text="{Binding DownSpeed, UpdateSourceTrigger=PropertyChanged}"/>
                             </StackPanel>
                             <Button Content="应用到所选" Grid.Row="1" Grid.Column="1" Command="{Binding SetIsChangeCommand}"/>
-                           
+
                         </Grid>
                     </StackPanel>
                 </TabItem>
@@ -273,11 +272,11 @@
                                 <ColumnDefinition/>
                             </Grid.ColumnDefinitions>
                             <TextBlock Margin="5" Text="降落延时"/>
-                           
+
                             <TextBox Margin="5" Grid.Column="2"
                                     Text="{Binding LandWaitTime, UpdateSourceTrigger=PropertyChanged}"/>
                             <Button Content="应用所选"  Command="{Binding SetSelLandWaitTimeCommand}" Grid.Column="3"/>
-                         
+
                         </Grid>
                     </StackPanel>
 
@@ -286,20 +285,50 @@
 
         </TabItem>
         <TabItem Header="灯光设计">
-            <StackPanel  DataContext="{Binding FlightTaskManager.SelectedTask.ModifyingSingleCopterInfo}">
+            <StackPanel  >
-                <StackPanel Visibility="Collapsed">
+                <StackPanel >
-                    <TextBlock Margin="5,5,0,0" Text="跑马灯"/>
+                    <StackPanel Orientation="Horizontal" Margin="0,5,0,5" >
+                        <TextBlock Margin="5,7,0,0" Text="起始时间"/>
+                        <TextBox Margin="5,5,0,3" Width="30" Text="{Binding BeginTime}"/>
+                        <TextBlock Margin="5,7,0,0" Text="结束时间"/>
+                        <TextBox Margin="5,5,0,3" Width="30" Text="{Binding EndTime}"/>
+                        <TextBlock Margin="5,7,0,0" Text="分层"/>
+                        <TextBox Margin="5,5,0,3" Width="30" Text="{Binding AverageSum}"/>
+                        <TextBlock Margin="5,7,0,0" Text="变换颜色"/>
+                        <TextBox Margin="5,5,0,3" Width="60" Text="{Binding ChangeRGB}"/>
+                        <TextBlock Margin="5,7,0,0" Text="结束颜色"/>
+                        <TextBox Margin="5,5,0,3" Width="60" Text="{Binding EndRGB}"/>
+                    </StackPanel>
+                    <Grid >
+                        <StackPanel Orientation="Horizontal">
+                            <TextBlock Margin="5,7,0,0" Text="起始时间"/>
+                            <TextBox Margin="5,5,0,3" Width="30" Text="{Binding StrokesTime}"/>
+                            <TextBlock Margin="5,7,0,0" Text="频率•"/>
+                            <TextBox Margin="5,5,0,3" Width="30" Text="{Binding IntervalTime}"/>
+                            <TextBlock Margin="5,7,0,0" Text="œ˜变换数量"/>
+                            <TextBox Margin="5,5,0,3" Width="30" Text="{Binding SingleNum}"/>
+                            <TextBlock Margin="5,7,0,0" Text="颜色"/>
+                            <TextBox Margin="5,5,0,3" Width="60" Text="{Binding StrokesRGB}"/>
+                            <Button Width="70" Content="灯光"
+                                Command="{Binding SetStrokesLampCommamd}" HorizontalAlignment="Right"/>
+                        </StackPanel>
+                        <Button Width="80" Content="跑马灯"
+                                Command="{Binding SetHorseRaceLampCommand}" HorizontalAlignment="Right"/>
+                    </Grid>
                     <Separator/>
                     <StackPanel Orientation="Horizontal">
-                        <TextBlock Margin="5,5,0,0" Text="起始时间"/>
+                        <TextBlock Margin="5,7,0,0" Text="时间"/>
-                        <TextBox Margin="5,3,0,3" Width="50"/>
+                        <TextBox Margin="5,5,0,3" Width="30" Text="{Binding GradientRampTime}"/>
-                        <TextBlock Margin="10,5,0,0" Text="结束时间"/>
+                        <TextBlock Margin="5,7,0,0" Text="起始色"/>
-                        <TextBox Margin="5,3,0,3" Width="50" />
+                        <TextBox Margin="5,5,0,3" Width="60" Text="{Binding LeftRGB}"/>
-                        <TextBlock Margin="10,5,0,0" Text="等分数量"/>
+                        <TextBlock Margin="5,7,0,0" Text="结束色"/>
-                        <TextBox Margin="5,3,0,3" Width="50" />
+                        <TextBox Margin="5,5,0,3" Width="60" Text="{Binding RightRGB}"/>
-                        <Button Margin="10,3,0,3" Width="80" Content="设置"/>
+                        <Button  Margin="10,5,0,3" Width="120" Content="设置渐变灯"
+                                Command="{Binding SetGradientRampCommand}" />
                     </StackPanel>
+                    <Separator/>
                 </StackPanel>
+                <StackPanel DataContext="{Binding FlightTaskManager.SelectedTask.ModifyingSingleCopterInfo}">
                 <Grid >
                     <Grid.ColumnDefinitions>
                         <ColumnDefinition/>
@@ -393,6 +422,7 @@
 
 
                 </ItemsControl>
+                </StackPanel>
             </StackPanel>
         </TabItem>
     </TabControl>

Plane.FormationCreator/Views/View3D.xaml.cs

@@ -26,6 +26,7 @@ namespace Plane.FormationCreator.Views
     public partial class View3D : UserControl
     {
         private FlightTaskManager _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
+        private View3DViewModel _view3DManager = ServiceLocator.Current.GetInstance<View3DViewModel>();
         public View3D()
         {
             InitializeComponent();
@@ -35,10 +36,11 @@ namespace Plane.FormationCreator.Views
             camera.LookDirection = new Vector3D(0, 1, 0);
             camera.UpDirection = new Vector3D(0, 0, 1);
             camera.FieldOfView = 120;
-
+            _flightTaskManager = ServiceLocator.Current.GetInstance<FlightTaskManager>();
-            this.DataContext = ServiceLocator.Current.GetInstance<View3DViewModel>();
+            _view3DManager = ServiceLocator.Current.GetInstance<View3DViewModel>();
-            view3d.ShowCameraInfo = true;
+            this.DataContext = _view3DManager;
-            view3d.ShowFieldOfView = true;
+            //view3d.ShowCameraInfo = true;
+            //view3d.ShowFieldOfView = true;
         }
 
 
@@ -55,6 +57,8 @@ namespace Plane.FormationCreator.Views
             camera.LookDirection = new Vector3D(0, 1, 0);
             camera.UpDirection = new Vector3D(0, 0, 1);
             camera.FieldOfView = 120;
+
+            _view3DManager.Clear3DCopters();
         }
     }
 }