Compare commits
23 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 8af733e644 | |||
| ffb95b2e7d | |||
| 361a8bf001 | |||
| 40bf208054 | |||
| a88311a160 | |||
| e909b7a3d3 | |||
| 3e3b6f08ee | |||
| 2045f87a83 | |||
| f82a285f8d | |||
| f0a4484cfc | |||
| 9c2238479f | |||
| f9814532f5 | |||
| c0f0f616dc | |||
| 14c489c016 | |||
| d051300171 | |||
| fc9b2595d6 | |||
| 14d1022775 | |||
| c72b6273b6 | |||
| f175def6a7 | |||
| 8f2cd98bd4 | |||
| d91b759d5f | |||
| f7be769b33 | |||
| 3e46bbb376 |
@ -11,7 +11,7 @@ namespace Plane.Copters
|
||||
Task CircleAsync();
|
||||
|
||||
/// <summary>
|
||||
/// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||
/// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||
/// </summary>
|
||||
/// <param name="bootloaderMode"></param>
|
||||
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
||||
|
||||
@ -34,15 +34,17 @@
|
||||
<WarningLevel>4</WarningLevel>
|
||||
<DocumentationFile>bin\Release\PlaneGcsSdk.Contract_Private.xml</DocumentationFile>
|
||||
</PropertyGroup>
|
||||
<ItemGroup>
|
||||
<!-- A reference to the entire .NET Framework is automatically included -->
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<Compile Include="Copters\DataStreamReceivedEventArgs.cs" />
|
||||
<Compile Include="Copters\DataStreamType.cs" />
|
||||
<Compile Include="Copters\ICopterActions.cs" />
|
||||
<Compile Include="Properties\AssemblyInfo.cs" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<Reference Include="System.Drawing">
|
||||
<HintPath>C:\Program Files (x86)\Reference Assemblies\Microsoft\Framework\.NETFramework\v4.8\System.Drawing.dll</HintPath>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
<Import Project="..\PlaneGcsSdk.Contract_Shared\PlaneGcsSdk.Contract_Shared.projitems" Label="Shared" />
|
||||
<Import Project="$(MSBuildExtensionsPath32)\Microsoft\Portable\$(TargetFrameworkVersion)\Microsoft.Portable.CSharp.targets" />
|
||||
<!-- To modify your build process, add your task inside one of the targets below and uncomment it.
|
||||
|
||||
@ -23,5 +23,10 @@
|
||||
/// 最高速度，单位为 m/s。
|
||||
/// </summary>
|
||||
public const float MAX_VEL = 5f;
|
||||
|
||||
public const float MAX_VEL_XY = 5.0f; //米/秒 水平速度
|
||||
public const float MAX_VEL_UP = 2.5f; //上升速度
|
||||
public const float MAX_VEL_DOWN = 1.5f; //下降速度
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -31,7 +31,9 @@ namespace Plane.Copters
|
||||
/// <param name="lng">经度。</param>
|
||||
/// <param name="alt">相对于解锁点的高度。</param>
|
||||
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
||||
Task FlyToAsync(double lat, double lng, float alt);
|
||||
/// flytime =飞行时间 秒
|
||||
/// flytype 0;匀速 1;加减速 2;单加速 3;单减速
|
||||
Task FlyToAsync(double lat, double lng, float alt,float flytime=0,int flytype = 0);
|
||||
|
||||
/// <summary>
|
||||
/// 切换到 <see cref="CopterState.HoverMode"/> 并悬停。此操作需要使用 GPS 定位，卫星数不足导致定位不准时非常危险。
|
||||
|
||||
@ -14,5 +14,13 @@ namespace Plane.Copters
|
||||
/// </summary>
|
||||
float GroundAlt { get; set; }
|
||||
float RetainInt{ get; }
|
||||
//模拟飞行更新间隔
|
||||
int sim_update_int { get; set; }
|
||||
float maxspeed_xy { get; set; }
|
||||
float maxspeed_up { get; set; }
|
||||
float maxspeed_down { get; set; }
|
||||
float acc_z { get; set; }
|
||||
float acc_xy { get; set; }
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
using Plane.Geography;
|
||||
using System;
|
||||
using System.Drawing;
|
||||
|
||||
namespace Plane.Copters
|
||||
{
|
||||
@ -187,5 +188,19 @@ namespace Plane.Copters
|
||||
/// LED颜色
|
||||
/// </summary>
|
||||
string LEDColor { get; set; }
|
||||
/// <summary>
|
||||
/// LED灯光模式 0常亮 1闪烁 2随机闪烁(RGB无意义)
|
||||
/// </summary>
|
||||
int LEDMode { get; set; }
|
||||
/// <summary>
|
||||
/// LED变化间隔
|
||||
/// </summary>
|
||||
float LEDInterval { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// LED显示颜色---内部计算后的真实颜色，用于显示
|
||||
/// </summary>
|
||||
Color LEDShowColor { get; set; }
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -12,7 +12,7 @@ namespace Plane.Copters
|
||||
double? latitude = null, //23.14973333,
|
||||
double? longitude = null, //113.40974166,
|
||||
float? altitude = null, //0,
|
||||
string name = null, //"林俊清的飞行器",
|
||||
string name = null, //"王海的飞行器",
|
||||
byte? batteryPer = null, //10,
|
||||
short? heading = null, //33,
|
||||
bool? isConnected = null, //true,
|
||||
|
||||
@ -55,6 +55,45 @@ namespace Plane.Geography
|
||||
{
|
||||
return CalcDistance(l1.Latitude, l1.Longitude, l1.Altitude, l2.Latitude, l2.Longitude, l2.Altitude);
|
||||
}
|
||||
/// <summary>
|
||||
/// 计算空间中两点间的距离，单位为米。
|
||||
/// </summary>
|
||||
/// <param name="lat1">纬度 1。</param>
|
||||
/// <param name="lng1">经度 1。</param>
|
||||
/// <param name="lat2">纬度 2。</param>
|
||||
/// <param name="lng2">经度 2。</param>
|
||||
/// <returns>空间中两点间的距离，单位为米。</returns>
|
||||
public static double CalcDistance_simple(double lat1, double lng1, double lat2, double lng2)
|
||||
{
|
||||
double dx = lng2 - lng1;
|
||||
double dy = lat2 - lat1;
|
||||
double b = (lat1 + lat2) * 0.5;
|
||||
double Lx = (0.05 * b * b * b - 19.16 * b * b + 47.13 * b + 110966) * dx;
|
||||
double Ly = (17 * b + 110352) * dy;
|
||||
return Math.Sqrt(Lx * Lx + Ly * Ly);
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 计算空间中两点间的距离，单位为米。
|
||||
/// </summary>
|
||||
/// <param name="lat1">纬度 1。</param>
|
||||
/// <param name="lng1">经度 1。</param>
|
||||
/// <param name="alt1">高度 1。</param>
|
||||
/// <param name="lat2">纬度 2。</param>
|
||||
/// <param name="lng2">经度 2。</param>
|
||||
/// <param name="alt2">高度 2。</param>
|
||||
/// <returns>空间中两点间的距离，单位为米。</returns>
|
||||
public static double CalcDistance_simple(double lat1, double lng1, double alt1, double lat2, double lng2,double alt2)
|
||||
{
|
||||
double dx = lng2 - lng1;
|
||||
double dy = lat2 - lat1;
|
||||
double b = (lat1 + lat2) * 0.5;
|
||||
double Lx = (0.05 * b * b * b - 19.16 * b * b + 47.13 * b + 110966) * dx;
|
||||
double Ly = (17 * b + 110352) * dy;
|
||||
double d= Math.Sqrt(Lx * Lx + Ly * Ly);
|
||||
return Math.Sqrt(Math.Pow((alt2 - alt1), 2) + Math.Pow(d, 2));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算空间中两点之间的距离，单位为米。
|
||||
|
||||
@ -6,7 +6,7 @@ namespace Plane.Copters
|
||||
public partial class FakeCopter
|
||||
{
|
||||
/// <summary>
|
||||
/// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||
/// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||
/// </summary>
|
||||
/// <param name="bootloaderMode"></param>
|
||||
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
||||
|
||||
@ -10,7 +10,7 @@ namespace Plane.Copters
|
||||
public PlaneCopter InternalCopter { get { return _internalCopter; } }
|
||||
|
||||
/// <summary>
|
||||
/// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||
/// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||
/// </summary>
|
||||
/// <param name="bootloaderMode"></param>
|
||||
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
||||
|
||||
@ -6,7 +6,7 @@ namespace Plane.Copters
|
||||
partial class PlaneCopter
|
||||
{
|
||||
/// <summary>
|
||||
/// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||
/// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||
/// </summary>
|
||||
/// <param name="bootloaderMode"></param>
|
||||
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
||||
|
||||
@ -1,79 +1,81 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project ToolsVersion="14.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
|
||||
<PropertyGroup>
|
||||
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
|
||||
<Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
|
||||
<ProjectGuid>{0111EB6E-72E3-499C-A3BA-022F5BBC4CAF}</ProjectGuid>
|
||||
<OutputType>Library</OutputType>
|
||||
<AppDesignerFolder>Properties</AppDesignerFolder>
|
||||
<RootNamespace>Plane</RootNamespace>
|
||||
<AssemblyName>PlaneGcsSdk_Private_NET46</AssemblyName>
|
||||
<TargetFrameworkVersion>v4.6</TargetFrameworkVersion>
|
||||
<FileAlignment>512</FileAlignment>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
|
||||
<DebugSymbols>true</DebugSymbols>
|
||||
<DebugType>full</DebugType>
|
||||
<Optimize>false</Optimize>
|
||||
<OutputPath>bin\Debug\</OutputPath>
|
||||
<DefineConstants>TRACE;DEBUG;PRIVATE</DefineConstants>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<WarningLevel>4</WarningLevel>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
|
||||
<DebugType>pdbonly</DebugType>
|
||||
<Optimize>true</Optimize>
|
||||
<OutputPath>bin\Release\</OutputPath>
|
||||
<DefineConstants>TRACE;PRIVATE</DefineConstants>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<WarningLevel>4</WarningLevel>
|
||||
<DocumentationFile>bin\Release\PlaneGcsSdk_Private_NET46.xml</DocumentationFile>
|
||||
</PropertyGroup>
|
||||
<ItemGroup>
|
||||
<Reference Include="PresentationCore" />
|
||||
<Reference Include="System" />
|
||||
<Reference Include="System.Core" />
|
||||
<Reference Include="System.Xml.Linq" />
|
||||
<Reference Include="System.Data.DataSetExtensions" />
|
||||
<Reference Include="Microsoft.CSharp" />
|
||||
<Reference Include="System.Data" />
|
||||
<Reference Include="System.Net.Http" />
|
||||
<Reference Include="System.Xml" />
|
||||
<Reference Include="WindowsBase" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\CopterImplSharedPart.Private.cs">
|
||||
<Link>Copters\CopterImplSharedPart.Private.cs</Link>
|
||||
</Compile>
|
||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PlaneCopter.Private.cs">
|
||||
<Link>Copters\PlaneCopter.Private.cs</Link>
|
||||
</Compile>
|
||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PLCopter.Private.cs">
|
||||
<Link>Copters\EHCopter.Private.cs</Link>
|
||||
</Compile>
|
||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\FakeCopter.Private.cs">
|
||||
<Link>Copters\FakeCopter.Private.cs</Link>
|
||||
</Compile>
|
||||
<Compile Include="Properties\AssemblyInfo.cs" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\..\Plane.Libraries\Plane.Windows.Messages\Plane.Windows.Messages.csproj">
|
||||
<Project>{413C18E2-235F-4E15-B5C1-633657DE5D7A}</Project>
|
||||
<Name>Plane.Windows.Messages</Name>
|
||||
</ProjectReference>
|
||||
<ProjectReference Include="..\PlaneGcsSdk.Contract_Private\PlaneGcsSdk.Contract_Private.csproj">
|
||||
<Project>{47141894-ece3-48ca-8dcf-ca751bda231e}</Project>
|
||||
<Name>PlaneGcsSdk.Contract_Private</Name>
|
||||
</ProjectReference>
|
||||
</ItemGroup>
|
||||
<Import Project="..\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.projitems" Label="Shared" />
|
||||
<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>
|
||||
-->
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project ToolsVersion="14.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
|
||||
<PropertyGroup>
|
||||
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
|
||||
<Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
|
||||
<ProjectGuid>{0111EB6E-72E3-499C-A3BA-022F5BBC4CAF}</ProjectGuid>
|
||||
<OutputType>Library</OutputType>
|
||||
<AppDesignerFolder>Properties</AppDesignerFolder>
|
||||
<RootNamespace>Plane</RootNamespace>
|
||||
<AssemblyName>PlaneGcsSdk_Private_NET46</AssemblyName>
|
||||
<TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
|
||||
<FileAlignment>512</FileAlignment>
|
||||
<TargetFrameworkProfile />
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
|
||||
<DebugSymbols>true</DebugSymbols>
|
||||
<DebugType>full</DebugType>
|
||||
<Optimize>false</Optimize>
|
||||
<OutputPath>bin\Debug\</OutputPath>
|
||||
<DefineConstants>TRACE;DEBUG;PRIVATE</DefineConstants>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<WarningLevel>4</WarningLevel>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
|
||||
<DebugType>pdbonly</DebugType>
|
||||
<Optimize>true</Optimize>
|
||||
<OutputPath>bin\Release\</OutputPath>
|
||||
<DefineConstants>TRACE;PRIVATE</DefineConstants>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<WarningLevel>4</WarningLevel>
|
||||
<DocumentationFile>bin\Release\PlaneGcsSdk_Private_NET46.xml</DocumentationFile>
|
||||
</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.Net.Http" />
|
||||
<Reference Include="System.Xml" />
|
||||
<Reference Include="WindowsBase" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\CopterImplSharedPart.Private.cs">
|
||||
<Link>Copters\CopterImplSharedPart.Private.cs</Link>
|
||||
</Compile>
|
||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PlaneCopter.Private.cs">
|
||||
<Link>Copters\PlaneCopter.Private.cs</Link>
|
||||
</Compile>
|
||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PLCopter.Private.cs">
|
||||
<Link>Copters\EHCopter.Private.cs</Link>
|
||||
</Compile>
|
||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\FakeCopter.Private.cs">
|
||||
<Link>Copters\FakeCopter.Private.cs</Link>
|
||||
</Compile>
|
||||
<Compile Include="Properties\AssemblyInfo.cs" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\..\Plane.Libraries\Plane.Windows.Messages\Plane.Windows.Messages.csproj">
|
||||
<Project>{413C18E2-235F-4E15-B5C1-633657DE5D7A}</Project>
|
||||
<Name>Plane.Windows.Messages</Name>
|
||||
</ProjectReference>
|
||||
<ProjectReference Include="..\PlaneGcsSdk.Contract_Private\PlaneGcsSdk.Contract_Private.csproj">
|
||||
<Project>{47141894-ece3-48ca-8dcf-ca751bda231e}</Project>
|
||||
<Name>PlaneGcsSdk.Contract_Private</Name>
|
||||
</ProjectReference>
|
||||
</ItemGroup>
|
||||
<Import Project="..\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.projitems" Label="Shared" />
|
||||
<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>
|
||||
@ -1,116 +1,138 @@
|
||||
#if !NETFX_CORE
|
||||
|
||||
using System;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace Plane.Communication
|
||||
{
|
||||
/// <summary>
|
||||
/// 提供作为 TCP 客户端通信的能力。
|
||||
/// </summary>
|
||||
public class TcpConnection : TcpConnectionBase
|
||||
{
|
||||
private string _remoteHostname;
|
||||
|
||||
private int _remotePort;
|
||||
|
||||
public TcpConnection(string remoteHostname, int remotePort = 5250)
|
||||
{
|
||||
_remoteHostname = remoteHostname;
|
||||
_remotePort = remotePort;
|
||||
_client = new TcpClient
|
||||
{
|
||||
ReceiveBufferSize = 40 * 1024,
|
||||
ReceiveTimeout = 1200
|
||||
};
|
||||
}
|
||||
|
||||
public void Open()
|
||||
{
|
||||
if (!IsOpen)
|
||||
{
|
||||
try
|
||||
{
|
||||
_client.Connect(_remoteHostname, _remotePort);
|
||||
}
|
||||
catch (SocketException)
|
||||
{
|
||||
CreateClientAndConnect();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
CreateClientAndConnect();
|
||||
}
|
||||
_isBroken = false;
|
||||
}
|
||||
|
||||
_stream = _client.GetStream();
|
||||
}
|
||||
|
||||
public async Task<bool> BindIP(string ip)
|
||||
{
|
||||
bool bind = false;
|
||||
try
|
||||
{
|
||||
IPAddress IPLocal = IPAddress.Parse(ip);
|
||||
_client.Client.Bind(new IPEndPoint(IPLocal, 0));
|
||||
bind = true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
bind = false;
|
||||
}
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
return bind;
|
||||
}
|
||||
|
||||
public override async Task OpenAsync()
|
||||
{
|
||||
string logstr;
|
||||
if (!IsOpen)
|
||||
{
|
||||
try
|
||||
{
|
||||
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
|
||||
}
|
||||
//屏蔽掉异常处理
|
||||
//CreateClientAndConnectAsync会new一个TcpClient并且重新连接
|
||||
//之前设置的TcpClient中Socket Bind会无效，在多网卡工作时会导致断线重连的时间特别长
|
||||
catch (SocketException e)
|
||||
{
|
||||
logstr= e.Message;
|
||||
//await CreateClientAndConnectAsync();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
//await CreateClientAndConnectAsync();
|
||||
}
|
||||
_isBroken = false;
|
||||
}
|
||||
_stream = _client.GetStream();
|
||||
}
|
||||
|
||||
private void CreateClientAndConnect()
|
||||
{
|
||||
_client = new TcpClient(_remoteHostname, _remotePort)
|
||||
{
|
||||
ReceiveBufferSize = 40 * 1024,
|
||||
ReceiveTimeout = 1200
|
||||
};
|
||||
}
|
||||
|
||||
private async Task CreateClientAndConnectAsync()
|
||||
{
|
||||
_client = new TcpClient
|
||||
{
|
||||
ReceiveBufferSize = 40 * 1024,
|
||||
ReceiveTimeout = 1200
|
||||
};
|
||||
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
#if !NETFX_CORE
|
||||
|
||||
using System;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace Plane.Communication
|
||||
{
|
||||
/// <summary>
|
||||
/// 提供作为 TCP 客户端通信的能力。
|
||||
/// </summary>
|
||||
public class TcpConnection : TcpConnectionBase
|
||||
{
|
||||
private string _remoteHostname;
|
||||
const int tcpReceiveBufferSize = 20 * 1024; //40 * 1024,
|
||||
const int tcpReceiveTimeout = 1200;
|
||||
|
||||
private int _remotePort;
|
||||
|
||||
public TcpConnection(string remoteHostname, int remotePort = 5250)
|
||||
{
|
||||
_remoteHostname = remoteHostname;
|
||||
_remotePort = remotePort;
|
||||
_client = new TcpClient
|
||||
{
|
||||
ReceiveBufferSize = tcpReceiveBufferSize,
|
||||
ReceiveTimeout = tcpReceiveTimeout
|
||||
};
|
||||
}
|
||||
|
||||
public void Open()
|
||||
{
|
||||
if (!IsOpen)
|
||||
{
|
||||
try
|
||||
{
|
||||
_client.Connect(_remoteHostname, _remotePort);
|
||||
}
|
||||
catch (SocketException)
|
||||
{
|
||||
CreateClientAndConnect();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
CreateClientAndConnect();
|
||||
}
|
||||
_isBroken = false;
|
||||
}
|
||||
|
||||
_stream = _client.GetStream();
|
||||
}
|
||||
|
||||
public async Task<bool> BindIP(string ip)
|
||||
{
|
||||
bool bind = false;
|
||||
try
|
||||
{
|
||||
IPAddress IPLocal = IPAddress.Parse(ip);
|
||||
_client.Client.Bind(new IPEndPoint(IPLocal, 0));
|
||||
bind = true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
bind = false;
|
||||
}
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
return bind;
|
||||
}
|
||||
|
||||
public override async Task OpenAsync()
|
||||
{
|
||||
string logstr;
|
||||
if (!IsOpen)
|
||||
{
|
||||
if (_client == null)
|
||||
CreateClientAndConnect();
|
||||
try
|
||||
{
|
||||
var connectTask = _client.ConnectAsync(_remoteHostname, _remotePort);
|
||||
if (await Task.WhenAny(connectTask, Task.Delay(5000)) == connectTask)
|
||||
{
|
||||
if (_client.Client != null) //需要测试
|
||||
await connectTask.ConfigureAwait(false);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Connection timed out
|
||||
throw new TimeoutException("Connection timed out");
|
||||
}
|
||||
}
|
||||
catch (SocketException e)
|
||||
{
|
||||
logstr = e.Message;
|
||||
CloseClient(); // 关闭并清理客户端
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
CloseClient(); // 关闭并清理客户端
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
CloseClient(); // 处理其他可能的异常
|
||||
// throw; // 可选：重新抛出异常，以便调用者知道发生了错误
|
||||
}
|
||||
_isBroken = false;
|
||||
}
|
||||
if (_client != null)
|
||||
_stream = _client.GetStream();
|
||||
}
|
||||
private void CloseClient()
|
||||
{
|
||||
_client?.Close(); // 如果 _client 不为 null，则关闭连接
|
||||
_client = null; // 将 _client 设置为 null，以便垃圾回收器可以回收它
|
||||
}
|
||||
|
||||
private void CreateClientAndConnect()
|
||||
{
|
||||
_client = new TcpClient(_remoteHostname, _remotePort)
|
||||
{
|
||||
ReceiveBufferSize = tcpReceiveBufferSize,
|
||||
ReceiveTimeout = tcpReceiveTimeout
|
||||
};
|
||||
}
|
||||
|
||||
private async Task CreateClientAndConnectAsync()
|
||||
{
|
||||
_client = new TcpClient
|
||||
{
|
||||
ReceiveBufferSize = tcpReceiveBufferSize,// 40 * 1024,
|
||||
ReceiveTimeout = tcpReceiveTimeout// 1200
|
||||
};
|
||||
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@ -43,7 +43,10 @@ namespace Plane.Communication
|
||||
{
|
||||
try
|
||||
{
|
||||
return _client.Client != null && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
|
||||
// bool bret ;
|
||||
// bret = _client.Client != null;
|
||||
// bret = bret && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
|
||||
return _client != null && _client.Client != null && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
|
||||
@ -21,7 +21,7 @@ namespace Plane.Communication
|
||||
|
||||
private bool _isListening;
|
||||
|
||||
// TODO: 林俊清, 20150909, 从配置文件中获取要监听的 IP 地址和端口号。
|
||||
// TODO: 王海, 20150909, 从配置文件中获取要监听的 IP 地址和端口号。
|
||||
private TcpListener _listener;
|
||||
|
||||
private bool _shouldListen;
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -1,205 +1,213 @@
|
||||
using Plane.Communication;
|
||||
using Plane.Protocols;
|
||||
using Plane.Windows.Messages;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace Plane.CommunicationManagement
|
||||
{
|
||||
public partial class CommModuleManager
|
||||
{
|
||||
private int MissionStartCopterId = 0;
|
||||
private int MissionSendCopterId = 0;
|
||||
private int MissionErrorId = -1;
|
||||
private int ErrorCode = 0;
|
||||
public event EventHandler<CommunicationReceiveCopterInfoEventArgs> CommunicationReceived;
|
||||
public event EventHandler<CommunicationCopterDisconnectEventArgs> CommunicationCopterDisconnect;
|
||||
public event EventHandler<CommunicationConnectEventArgs> CommunicationConnected;
|
||||
private Dictionary<int, CommWriteMissinState> missionWriteState = new Dictionary<int, CommWriteMissinState>();
|
||||
private static readonly object MissinLocker = new object();
|
||||
|
||||
public Dictionary<int, CommWriteMissinState> MissionWriteState
|
||||
{
|
||||
get {return missionWriteState; }
|
||||
}
|
||||
|
||||
public void ClearMissionWriteState()
|
||||
{
|
||||
missionWriteState.Clear();
|
||||
}
|
||||
|
||||
private void AnalyzeMissionStartPacket(short copterId, byte[] buffer)
|
||||
{
|
||||
short errorId = BitConverter.ToInt16(buffer, 0);
|
||||
MissionStartCopterId = copterId;
|
||||
MissionErrorId = errorId;
|
||||
if(errorId != 0)
|
||||
Message.Show($"{copterId}:返回 = {errorId}");
|
||||
}
|
||||
|
||||
private void AnalyzeStringPacket(short copterId, byte[] buffer)
|
||||
{
|
||||
int count = Array.IndexOf(buffer, (byte)0);
|
||||
string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube");
|
||||
Message.Show(msg);
|
||||
}
|
||||
|
||||
private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer)
|
||||
{
|
||||
ushort ret = BitConverter.ToUInt16(buffer, 0);
|
||||
switch (ret)
|
||||
{
|
||||
case MavComm.SEARCH_FINDCOPTER:
|
||||
Message.Show(copterId.ToString() + "---飞机开始相应");
|
||||
break;
|
||||
|
||||
case MavComm.SEARCH_COMPLETED:
|
||||
Message.Show(copterId.ToString() + "---设置成功");
|
||||
break;
|
||||
|
||||
case MavComm.SEARCH_OUTTIME:
|
||||
Message.Show("超时无响应");
|
||||
break;
|
||||
|
||||
case MavComm.MISSION_SEND_COMPLETED:
|
||||
MissionSendCopterId = copterId;
|
||||
break;
|
||||
|
||||
case MavComm.P2P_SEND_FAILED:
|
||||
Message.Show("点对点通信发送失败");
|
||||
break;
|
||||
default:
|
||||
if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END)
|
||||
{
|
||||
ErrorCode = ret;
|
||||
Message.Show($"{copterId}--错误码：{ret}");
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer)
|
||||
{
|
||||
byte type = buffer[0];
|
||||
byte connectRet;
|
||||
switch (type)
|
||||
{
|
||||
case 0x01:
|
||||
//connectRet = buffer[1];
|
||||
//if (connectRet == 0x0) //飞机连接上 不管飞控和通信模块的连接
|
||||
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
|
||||
break;
|
||||
case 0x02:
|
||||
connectRet = buffer[1];
|
||||
if (connectRet == 0x0) //飞机断开
|
||||
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
||||
break;
|
||||
|
||||
case 0x03:
|
||||
SaveMissionWriteStat(copterId, buffer[1]);
|
||||
break;
|
||||
|
||||
case 0x0e:
|
||||
if (copterId == 0)
|
||||
Message.Show("----------全部更新完成----------");
|
||||
else
|
||||
Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
private void SaveMissionWriteStat(short copterId, byte wirteMissRet)
|
||||
{
|
||||
Task.Run(async () =>
|
||||
{
|
||||
lock (MissinLocker)
|
||||
{
|
||||
// var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]);
|
||||
|
||||
if (wirteMissRet == 0x20)
|
||||
{
|
||||
if (missionWriteState.ContainsKey(copterId))
|
||||
missionWriteState[copterId].WriteSucceed = true;
|
||||
Message.Show($"ID:{copterId}:航点写入成功");
|
||||
}
|
||||
if (wirteMissRet > 0x20 && wirteMissRet < 0x30)
|
||||
{
|
||||
if (missionWriteState.ContainsKey(copterId))
|
||||
missionWriteState[copterId].WriteSucceed = false;
|
||||
Message.Show($"ID:{copterId}:航点写入失败");
|
||||
}
|
||||
}
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
}
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
private void AnalyzeCopterInfosPacket(byte[] buffer)
|
||||
{
|
||||
ushort beginNum = BitConverter.ToUInt16(buffer, 0);
|
||||
ushort endNum = BitConverter.ToUInt16(buffer, 2);
|
||||
int count = endNum - beginNum + 1;
|
||||
byte[] copter_packets = buffer.Skip(4).ToArray();
|
||||
if (copter_packets.Length != count * 4)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
int offset = 0;
|
||||
for (int i = beginNum; i <= endNum; i++)
|
||||
{
|
||||
byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray();
|
||||
UInt16 state = BitConverter.ToUInt16(onePacket, 0);
|
||||
short copterId = (short)i;
|
||||
switch (state >> 13)
|
||||
{
|
||||
//0B000
|
||||
case 0x0000 >> 13:
|
||||
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
||||
break;
|
||||
//0B100
|
||||
case 0x8000 >> 13:
|
||||
break;
|
||||
//0B110
|
||||
case 0xC000 >> 13:
|
||||
//0B111
|
||||
case 0xE000 >> 13:
|
||||
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
|
||||
break;
|
||||
}
|
||||
offset += 4;
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer)
|
||||
{
|
||||
byte[] packet = buffer.Take(28).ToArray();
|
||||
byte[] state_packet = buffer.Skip(28).ToArray();
|
||||
UInt16 state = BitConverter.ToUInt16(state_packet,0);
|
||||
byte version = state_packet[3];
|
||||
switch (state >> 13)
|
||||
{
|
||||
//0B000
|
||||
case 0x0000 >> 13:
|
||||
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
||||
break;
|
||||
//0B100
|
||||
case 0x8000 >> 13:
|
||||
break;
|
||||
//0B110
|
||||
case 0xC000 >> 13:
|
||||
//0B111
|
||||
case 0xE000 >> 13:
|
||||
CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version));
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
using Plane.Communication;
|
||||
using Plane.Protocols;
|
||||
using Plane.Windows.Messages;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace Plane.CommunicationManagement
|
||||
{
|
||||
public partial class CommModuleManager
|
||||
{
|
||||
private int MissionStartCopterId = 0;
|
||||
private int MissionSendCopterId = 0;
|
||||
private int MissionErrorId = -1;
|
||||
private int ErrorCode = 0;
|
||||
public event EventHandler<CommunicationReceiveCopterInfoEventArgs> CommunicationReceived;
|
||||
public event EventHandler<CommunicationCopterDisconnectEventArgs> CommunicationCopterDisconnect;
|
||||
public event EventHandler<CommunicationConnectEventArgs> CommunicationConnected;
|
||||
private Dictionary<int, CommWriteMissinState> missionWriteState = new Dictionary<int, CommWriteMissinState>();
|
||||
private static readonly object MissinLocker = new object();
|
||||
|
||||
public Dictionary<int, CommWriteMissinState> MissionWriteState
|
||||
{
|
||||
get {return missionWriteState; }
|
||||
}
|
||||
|
||||
public void ClearMissionWriteState()
|
||||
{
|
||||
missionWriteState.Clear();
|
||||
}
|
||||
|
||||
private void AnalyzeMissionStartPacket(short copterId, byte[] buffer)
|
||||
{
|
||||
short errorId = BitConverter.ToInt16(buffer, 0);
|
||||
MissionStartCopterId = copterId;
|
||||
MissionErrorId = errorId;
|
||||
if(errorId != 0)
|
||||
Message.Show($"{copterId}:返回 = {errorId}");
|
||||
}
|
||||
|
||||
private void AnalyzeStringPacket(short copterId, byte[] buffer)
|
||||
{
|
||||
int count = Array.IndexOf(buffer, (byte)0);
|
||||
string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube");
|
||||
Message.Show(msg);
|
||||
}
|
||||
|
||||
private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer)
|
||||
{
|
||||
ushort ret = BitConverter.ToUInt16(buffer, 0);
|
||||
switch (ret)
|
||||
{
|
||||
case MavComm.SEARCH_FINDCOPTER:
|
||||
Message.Show(copterId.ToString() + "---飞机开始相应");
|
||||
break;
|
||||
|
||||
case MavComm.SEARCH_COMPLETED:
|
||||
Message.Show(copterId.ToString() + "---设置成功");
|
||||
break;
|
||||
|
||||
case MavComm.SEARCH_OUTTIME:
|
||||
Message.Show("超时无响应");
|
||||
break;
|
||||
|
||||
case MavComm.MISSION_SEND_COMPLETED:
|
||||
MissionSendCopterId = copterId;
|
||||
break;
|
||||
|
||||
case MavComm.P2P_SEND_FAILED:
|
||||
Message.Show("点对点通信发送失败");
|
||||
break;
|
||||
default:
|
||||
if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END)
|
||||
{
|
||||
ErrorCode = ret;
|
||||
Message.Show($"{copterId}--错误码：{ret}");
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer)
|
||||
{
|
||||
byte type = buffer[0];
|
||||
byte connectRet;
|
||||
switch (type)
|
||||
{
|
||||
case 0x01:
|
||||
//connectRet = buffer[1];
|
||||
//if (connectRet == 0x0) //飞机连接上 不管飞控和通信模块的连接
|
||||
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
|
||||
break;
|
||||
case 0x02:
|
||||
connectRet = buffer[1];
|
||||
if (connectRet == 0x0) //飞机断开
|
||||
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
||||
break;
|
||||
|
||||
case 0x03:
|
||||
SaveMissionWriteStat(copterId, buffer[1]);
|
||||
break;
|
||||
|
||||
case 0x04:
|
||||
Message.Show($"飞机{copterId}:功率设置成功，新回传功率：{buffer[1]}");
|
||||
break;
|
||||
|
||||
case 0x0e:
|
||||
if (copterId == 0)
|
||||
Message.Show("----------全部更新完成----------");
|
||||
else
|
||||
Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
|
||||
break;
|
||||
|
||||
default:
|
||||
// Message.Show($"Comm3返回：{type}");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
private void SaveMissionWriteStat(short copterId, byte wirteMissRet)
|
||||
{
|
||||
Task.Run(async () =>
|
||||
{
|
||||
lock (MissinLocker)
|
||||
{
|
||||
// var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]);
|
||||
|
||||
if (wirteMissRet == 0x20)
|
||||
{
|
||||
if (missionWriteState.ContainsKey(copterId))
|
||||
missionWriteState[copterId].WriteSucceed = true;
|
||||
Message.Show($"ID:{copterId}:航点写入成功");
|
||||
}
|
||||
if (wirteMissRet > 0x20 && wirteMissRet < 0x30)
|
||||
{
|
||||
if (missionWriteState.ContainsKey(copterId))
|
||||
missionWriteState[copterId].WriteSucceed = false;
|
||||
Message.Show($"ID:{copterId}:航点写入失败");
|
||||
}
|
||||
}
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
}
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
private void AnalyzeCopterInfosPacket(byte[] buffer)
|
||||
{
|
||||
ushort beginNum = BitConverter.ToUInt16(buffer, 0);
|
||||
ushort endNum = BitConverter.ToUInt16(buffer, 2);
|
||||
int count = endNum - beginNum + 1;
|
||||
byte[] copter_packets = buffer.Skip(4).ToArray();
|
||||
if (copter_packets.Length != count * 4)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
int offset = 0;
|
||||
for (int i = beginNum; i <= endNum; i++)
|
||||
{
|
||||
byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray();
|
||||
UInt16 state = BitConverter.ToUInt16(onePacket, 0);
|
||||
short copterId = (short)i;
|
||||
switch (state >> 13)
|
||||
{
|
||||
//0B000
|
||||
case 0x0000 >> 13:
|
||||
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
||||
break;
|
||||
//0B100
|
||||
case 0x8000 >> 13:
|
||||
break;
|
||||
//0B110
|
||||
case 0xC000 >> 13:
|
||||
//0B111
|
||||
case 0xE000 >> 13:
|
||||
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
|
||||
break;
|
||||
}
|
||||
offset += 4;
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer)
|
||||
{
|
||||
byte[] packet = buffer.Take(28).ToArray();
|
||||
byte[] state_packet = buffer.Skip(28).ToArray();
|
||||
UInt16 state = BitConverter.ToUInt16(state_packet,0);
|
||||
byte version = state_packet[3];
|
||||
switch (state >> 13)
|
||||
{
|
||||
//0B000
|
||||
case 0x0000 >> 13:
|
||||
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
||||
break;
|
||||
//0B100
|
||||
case 0x8000 >> 13:
|
||||
break;
|
||||
//0B110
|
||||
case 0xC000 >> 13:
|
||||
//0B111
|
||||
case 0xE000 >> 13: //最后正在用的版本
|
||||
CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version));
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -63,7 +63,7 @@ namespace Plane.CopterControllers
|
||||
{
|
||||
if (_copterManager.Copter != null && _acceptingMobileControlStates.Contains(_copterManager.Copter.State))
|
||||
{
|
||||
// TODO: 林俊清, 20151109, 整理并使用 Plane.Copters.Constants 类中的常量。
|
||||
// TODO: 王海, 20151109, 整理并使用 Plane.Copters.Constants 类中的常量。
|
||||
|
||||
ushort maxChannelOffset = (ushort)(Speed == SpeedType.SpeedFast ? 400 : 200);
|
||||
|
||||
|
||||
355
PlaneGcsSdk_Shared/CopterControllers/GuidController.cs
Normal file
355
PlaneGcsSdk_Shared/CopterControllers/GuidController.cs
Normal file
@ -0,0 +1,355 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace Plane.CopterControllers
|
||||
{
|
||||
class GuidController
|
||||
{
|
||||
|
||||
float fabsf(float vvalue)
|
||||
{
|
||||
return Math.Abs(vvalue);
|
||||
|
||||
}
|
||||
float sqrt(float vvalue)
|
||||
{
|
||||
return (float)Math.Sqrt(vvalue);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 计算小航点飞行
|
||||
/// </summary>
|
||||
/// <param name="Distance"></param>
|
||||
/// <param name="fc_acc"></param>
|
||||
/// <param name="fc_maxspeed"></param>
|
||||
/// <returns></returns>
|
||||
|
||||
//单算减速，不算加速的最小飞行时间
|
||||
float getMinfligthtime_v2(float Distance, float fc_acc, float fc_maxspeed)
|
||||
{
|
||||
float fDis = fabsf(Distance); // 总距离
|
||||
float facc = fabsf(fc_acc); // 减速度
|
||||
|
||||
// 物体开始时即以最大速度运动，不考虑加速过程
|
||||
float vel = fc_maxspeed;
|
||||
|
||||
// 计算减速所需的时间和距离
|
||||
// 减速时间 (从最大速度减速到0)
|
||||
float dectime = vel / facc;
|
||||
// 减速阶段覆盖的距离
|
||||
float decdis = (vel * dectime) - (0.5f * facc * dectime * dectime);
|
||||
|
||||
// 判断是否有足够的距离进行减速
|
||||
if (decdis >= fDis)
|
||||
{
|
||||
// 如果减速所需距离已经超过或等于总距离，调整最大速度
|
||||
// 使用公式 v^2 = u^2 + 2as 解出 v
|
||||
vel = (float)Math.Sqrt(2 * facc * fDis);
|
||||
// 重新计算减速时间
|
||||
dectime = vel / facc;
|
||||
}
|
||||
|
||||
// 计算匀速阶段时间
|
||||
float unftime = 0.0f;
|
||||
if (decdis < fDis)
|
||||
{
|
||||
// 如果有剩余距离进行匀速运动
|
||||
unftime = (fDis - decdis) / vel;
|
||||
}
|
||||
|
||||
// 总飞行时间 = 匀速阶段时间 + 减速阶段时间
|
||||
return unftime + dectime;
|
||||
}
|
||||
|
||||
//单算减速，不算加速的最大飞行速度
|
||||
public float CalculateMaximumVelocity(float distance, float time, float acceleration)
|
||||
{
|
||||
// 二次方程系数
|
||||
float a_coeff = 1;
|
||||
float b_coeff = -2 * time;
|
||||
float c_coeff = (2 * distance) / acceleration;
|
||||
|
||||
// 计算判别式
|
||||
float discriminant = b_coeff * b_coeff - 4 * a_coeff * c_coeff;
|
||||
|
||||
if (discriminant < 0)
|
||||
{
|
||||
return -1; // 无实数解
|
||||
}
|
||||
|
||||
// 计算二次方程的根
|
||||
float t1_root1 = (-b_coeff + (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
|
||||
float t1_root2 = (-b_coeff - (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
|
||||
|
||||
// 选择合理的根作为 t1
|
||||
float t1 = Math.Min(t1_root1, t1_root2);
|
||||
if (t1 < 0 || t1 > time)
|
||||
{
|
||||
return -1; // 无合理解
|
||||
}
|
||||
|
||||
// 计算最大速度 v
|
||||
return acceleration * t1;
|
||||
}
|
||||
|
||||
//单算加速，不算减速的距离速度计算
|
||||
float getspeeddist_V2(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
|
||||
{
|
||||
vdist = 0;
|
||||
vspeed = 0;
|
||||
float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
|
||||
if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
|
||||
{
|
||||
vspeed = Distance / flight_time;
|
||||
vdist = vspeed * curr_time; //匀速距离
|
||||
return 0;
|
||||
|
||||
}
|
||||
float dect = desV / fc_acc; // 总加速时间
|
||||
float unit = flight_time - dect; //匀速段时间
|
||||
float decD = (fc_acc * dect * dect) / 2; //总加速距离
|
||||
|
||||
if (dect > flight_time) dect = flight_time;//约束时间
|
||||
if (curr_time > dect) //大于加速段时间--匀速
|
||||
{
|
||||
vspeed = (Distance - decD) / (flight_time - dect);
|
||||
vdist = vspeed * (curr_time - dect);
|
||||
vdist = vdist + decD; //总距离=匀速距离+减速距离
|
||||
if (vdist > Distance) vdist = Distance; //约束距离
|
||||
|
||||
}
|
||||
else //加速段
|
||||
{
|
||||
vspeed = fc_acc * curr_time;
|
||||
vdist = (fc_acc * curr_time * curr_time) / 2; //加速距离
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
//单算减速，不算加速的距离速度计算
|
||||
float getspeeddist_V3(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
|
||||
{
|
||||
vdist = 0;
|
||||
vspeed = 0;
|
||||
float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
|
||||
if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
|
||||
{
|
||||
vspeed = Distance / flight_time;
|
||||
vdist = vspeed * curr_time; //匀速距离
|
||||
return 0;
|
||||
|
||||
}
|
||||
float dect = desV / fc_acc; // 总减速时间
|
||||
float unit = flight_time - dect; //匀速段时间
|
||||
float decD = desV * dect - (fc_acc * dect * dect) / 2; //总减速距离
|
||||
|
||||
if (dect > flight_time) dect = flight_time;//约束时间
|
||||
//匀速时间内
|
||||
if (curr_time < unit)
|
||||
{
|
||||
vspeed = (Distance - decD) / (flight_time - dect);
|
||||
vdist = vspeed * curr_time;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (((flight_time - dect) * unit) == 0)
|
||||
vdist = 0;
|
||||
else
|
||||
vdist = (Distance - decD) / (flight_time - dect) * unit; //匀速距离
|
||||
float currdect = curr_time - unit; //减速时间
|
||||
if (currdect >= 0)
|
||||
{
|
||||
vspeed = desV - fc_acc * currdect;
|
||||
decD = desV * currdect - (fc_acc * currdect * currdect) / 2; //减速距离
|
||||
}
|
||||
vdist = vdist + decD; //总距离=匀速距离+减速距离
|
||||
if (vdist > Distance) vdist = Distance; //约束距离
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed)
|
||||
{
|
||||
float fDis = fabsf(Distance);
|
||||
float facc = fabsf(fc_acc);
|
||||
|
||||
float realflytime = 0.0f;
|
||||
//计算一半的距离
|
||||
float hafdis = (fDis / 2);
|
||||
//计算最大速度
|
||||
float vel = (float)sqrt(2 * facc * hafdis);
|
||||
//如果速度超过最大速度
|
||||
if (vel > fc_maxspeed)
|
||||
//使用最大速度
|
||||
vel = fc_maxspeed;
|
||||
//加速时间
|
||||
float acctim = vel / facc;
|
||||
//加速距离
|
||||
float accdis = vel * vel / (2 * facc);
|
||||
//匀速段时间
|
||||
float vtime = (hafdis - accdis) / vel;
|
||||
//到一半的时间
|
||||
float haftime = acctim + vtime;
|
||||
realflytime = haftime * 2;
|
||||
return realflytime;
|
||||
}
|
||||
|
||||
///计算飞行距离和速度 单位--米，秒----
|
||||
//返回 整个距离最大飞行速度
|
||||
///flight_time 总飞行时间 Distance 飞行总距离 curr_time 当前飞行时间 fc_acc加速度, fc_maxspeed最大速度 vdist 计算的应该飞的距离 vspeed 计算的当前时间应该速度
|
||||
//========这是飞控正在使用的算法========
|
||||
float getspeeddist_V1(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
|
||||
{
|
||||
float plan_flytime=_plan_flytime;
|
||||
float desired_velocity=_desired_velocity;
|
||||
float dist = 0;
|
||||
//导航加速度 米/秒*秒 ---不使用导航库
|
||||
float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
|
||||
//
|
||||
float speed = 0;
|
||||
//计算实时速度
|
||||
if (curr_time <= (plan_flytime / 2.0))
|
||||
speed = curr_time * wpacc;
|
||||
else
|
||||
//需要减速
|
||||
speed = (plan_flytime - curr_time) * wpacc;
|
||||
//不能大于目标速度
|
||||
if (speed > desired_velocity)
|
||||
speed = desired_velocity;
|
||||
if (speed <= 0)
|
||||
speed = 0;
|
||||
vspeed = speed;
|
||||
//计算实时距离
|
||||
float V_start = 0.0f;
|
||||
float V_end = 0.0f;
|
||||
//加速段
|
||||
float t1 = (desired_velocity - V_start) / wpacc;
|
||||
//减速段
|
||||
float t3 = (desired_velocity - V_end) / wpacc;
|
||||
//平飞段
|
||||
float t2 = plan_flytime - t1 - t3;
|
||||
if (curr_time < t1)
|
||||
{
|
||||
dist = 0.5f * wpacc * curr_time * curr_time + curr_time * V_start;
|
||||
}
|
||||
else if (curr_time < t1 + t2)
|
||||
{
|
||||
dist = -0.5f * wpacc * t1 * t1 + (curr_time) * desired_velocity;
|
||||
}
|
||||
else
|
||||
{
|
||||
float t = curr_time - t1 - t2;
|
||||
dist = -0.5f * wpacc * t1 * t1 + curr_time * desired_velocity - 0.5f * wpacc * t * t;
|
||||
}
|
||||
|
||||
if (fabsf(dist) > fabsf(Distance))
|
||||
vdist = Distance;
|
||||
else
|
||||
{
|
||||
if (Distance < 0)
|
||||
vdist = -dist;
|
||||
else vdist = dist;
|
||||
}
|
||||
|
||||
return desired_velocity;
|
||||
}
|
||||
|
||||
float _vspeed=0;
|
||||
|
||||
float _desired_velocity = 0;
|
||||
float _plan_flytime = 0;
|
||||
|
||||
float initgetspeeddist_V1(float flight_time, float Distance, float fc_acc, float fc_maxspeed)
|
||||
{
|
||||
//计划飞行时间
|
||||
float plan_flytime = flight_time;
|
||||
//计算距离用绝对值
|
||||
float fDis = fabsf(Distance);
|
||||
|
||||
//导航加速度 米/秒*秒 ---不使用导航库
|
||||
float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
|
||||
|
||||
//最大目标速度---米/秒
|
||||
float desired_velocity = 0;
|
||||
|
||||
|
||||
//计算最小飞行时间
|
||||
float minflytime = getMinfligthtime(Distance, fc_acc, fc_maxspeed);
|
||||
if (flight_time < minflytime)
|
||||
plan_flytime = minflytime;// + 0.1f;
|
||||
|
||||
|
||||
//根据总飞行时间和总距离计算飞行速度----起始和结束速度都是0，中间按匀加速和匀减速计算，没考虑加加速度
|
||||
float delta = (0.5f * plan_flytime) * (0.5f * plan_flytime) - fDis / wpacc;
|
||||
if (delta >= 0)
|
||||
{
|
||||
desired_velocity = (0.5f * plan_flytime - sqrt(delta)) / (1 / wpacc);
|
||||
if (desired_velocity > fc_maxspeed)
|
||||
{
|
||||
plan_flytime = minflytime;
|
||||
desired_velocity = fc_maxspeed;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
desired_velocity = (0.5f * plan_flytime) / (1 / wpacc);
|
||||
}
|
||||
if (desired_velocity < 0.2f) desired_velocity = 0.2f; //限制最小速度0.2米/秒
|
||||
if (desired_velocity > fc_maxspeed) desired_velocity = fc_maxspeed;//限制最大速度10米/秒，应该参数化
|
||||
|
||||
_desired_velocity = desired_velocity;
|
||||
_plan_flytime = plan_flytime;
|
||||
return desired_velocity;
|
||||
}
|
||||
|
||||
|
||||
public float initgetspeeddist(int flytype, float flight_time, float Distance, float fc_acc, float fc_maxspeed)
|
||||
{
|
||||
flytype = 0;
|
||||
switch (flytype)
|
||||
{
|
||||
case 0: //匀速
|
||||
_vspeed = Distance / flight_time;
|
||||
return 0;
|
||||
case 1: //同时计算加减速
|
||||
return initgetspeeddist_V1(flight_time, Distance, fc_acc, fc_maxspeed);
|
||||
case 2: //单加速
|
||||
return 0;
|
||||
case 3: //单减速
|
||||
return 0;
|
||||
default:
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
public float getspeeddist(int flytype, float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
|
||||
{
|
||||
flytype = 0;
|
||||
switch (flytype)
|
||||
{
|
||||
case 0: //匀速
|
||||
//case 1: //匀速
|
||||
vspeed = _vspeed;
|
||||
vdist = vspeed * curr_time;
|
||||
return 0;
|
||||
case 1: //同时计算加减速
|
||||
return getspeeddist_V1(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
|
||||
case 2: //单加速
|
||||
return getspeeddist_V2(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
|
||||
case 3: //单减速
|
||||
return getspeeddist_V3(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
|
||||
default:
|
||||
vspeed = 0;
|
||||
vdist = 0;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@ -39,7 +39,7 @@ namespace Plane.CopterControllers
|
||||
base.IsEnabled = value;
|
||||
if (value)
|
||||
{
|
||||
// TODO: 林俊清, 20160303, 修复 KeyboardController。
|
||||
// TODO: 王海, 20160303, 修复 KeyboardController。
|
||||
//_messenger.Register<KeyDownMessage>(this, OnKeyDown);
|
||||
//_messenger.Register<KeyUpMessage>(this, OnKeyUp);
|
||||
}
|
||||
|
||||
@ -104,7 +104,7 @@ namespace Plane.CopterManagement
|
||||
return Copter.FlyToAsync(lat, lng);
|
||||
}
|
||||
|
||||
public Task FlyToAsync(double lat, double lng, float alt)
|
||||
public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
|
||||
{
|
||||
return Copter.FlyToAsync(lat, lng, alt);
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,309 +1,317 @@
|
||||
using System;
|
||||
using static Plane.Protocols.MAVLink;
|
||||
|
||||
namespace Plane.Copters
|
||||
{
|
||||
public partial class PLCopter : CopterImplSharedPart
|
||||
{
|
||||
private bool _fetchingFirmwareVersion;
|
||||
|
||||
private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
|
||||
{
|
||||
IsConnected = _internalCopter.IsConnected;
|
||||
}
|
||||
|
||||
private void _internalCopter_GetLogDataEvent(string log)
|
||||
{
|
||||
//飞机消息日志，后面需要改为记录方式
|
||||
StatusText =Name+":"+log;
|
||||
var e = new MessageCreatedEventArgs { Message = StatusText };
|
||||
RaiseTextReceived(e);
|
||||
}
|
||||
|
||||
private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
|
||||
{
|
||||
switch (StreamType)
|
||||
{
|
||||
case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
|
||||
{
|
||||
SatCount = _internalCopter.satcount;
|
||||
GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
|
||||
GpsHdop = _internalCopter.gpshdop;
|
||||
Altitude = _internalCopter.gpsalt;
|
||||
Retain = BitConverter.GetBytes(_internalCopter.retain);
|
||||
|
||||
Voltage = _internalCopter.battery_voltage;
|
||||
CommModuleMode = (FlightMode)_internalCopter.commModuleMode;
|
||||
CommModuleVersion = _internalCopter.commModuleVersion;
|
||||
IsUnlocked = _internalCopter.isUnlocked;
|
||||
//Yaw = _internalCopter.yaw;
|
||||
Heading = _internalCopter.heading;
|
||||
HeartbeatCount++;
|
||||
|
||||
if (SatCount > 8)
|
||||
{
|
||||
IsGpsAccurate = true;
|
||||
RecordLat = _internalCopter.lat;
|
||||
RecordLng = _internalCopter.lng;
|
||||
}
|
||||
|
||||
Latitude = RecordLat;
|
||||
Longitude = RecordLng;
|
||||
UpdateFlightDataIfNeeded();
|
||||
RaiseLocationChangedIfNeeded();
|
||||
|
||||
RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
|
||||
|
||||
break;
|
||||
}
|
||||
case MAVLINK_MSG_ID_ATTITUDE: //无用
|
||||
{
|
||||
Roll = _internalCopter.roll;
|
||||
Pitch = _internalCopter.pitch;
|
||||
Yaw = _internalCopter.yaw;
|
||||
TimebootMs = _internalCopter.timebootms;
|
||||
RaiseAttitudeChanged();
|
||||
|
||||
break;
|
||||
}
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要
|
||||
{
|
||||
Channel1 = _internalCopter.ch1in;
|
||||
Channel2 = _internalCopter.ch2in;
|
||||
Channel3 = _internalCopter.ch3in;
|
||||
Channel4 = _internalCopter.ch4in;
|
||||
Channel5 = _internalCopter.ch5in;
|
||||
Channel6 = _internalCopter.ch6in;
|
||||
Channel7 = _internalCopter.ch7in;
|
||||
Channel8 = _internalCopter.ch8in;
|
||||
|
||||
RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);
|
||||
|
||||
break;
|
||||
}
|
||||
case MAVLINK_MSG_ID_VFR_HUD:
|
||||
{
|
||||
Heading = _internalCopter.heading; //有用
|
||||
Altitude = _internalCopter.alt; //有用
|
||||
AirSpeed = _internalCopter.airspeed; //没用
|
||||
GroundSpeed = _internalCopter.groundspeed; //没用
|
||||
|
||||
RaiseAltitudeChangedIfNeeded();
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
|
||||
{
|
||||
IsUnlocked = _internalCopter.armed;
|
||||
Mode = (FlightMode)_internalCopter.mode;
|
||||
// 林俊清, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
|
||||
HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
|
||||
++HeartbeatCount;
|
||||
|
||||
IsCheckingConnection = false;
|
||||
/*
|
||||
if (FirmwareVersion == null && !_fetchingFirmwareVersion)
|
||||
{
|
||||
try
|
||||
{
|
||||
_fetchingFirmwareVersion = true;
|
||||
FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
|
||||
}
|
||||
catch (TimeoutException)
|
||||
{
|
||||
// 吞掉。
|
||||
}
|
||||
finally
|
||||
{
|
||||
_fetchingFirmwareVersion = false;
|
||||
}
|
||||
}
|
||||
*/
|
||||
RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
|
||||
}
|
||||
|
||||
private void _internalCopter_ReceiveSensorEvent(object sender)
|
||||
{
|
||||
RaiseSensorDataReceived(EventArgs.Empty);
|
||||
}
|
||||
|
||||
private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff)
|
||||
{
|
||||
Voltage = _internalCopter.battery_voltage / 1000;
|
||||
|
||||
if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。
|
||||
{
|
||||
CalcBatteryPer();
|
||||
}
|
||||
else
|
||||
{
|
||||
BatteryPer = _internalCopter.battery_remaining;
|
||||
}
|
||||
|
||||
const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308;
|
||||
const int MIN_VERSION_OF_GENERATION2 = 0x3000;
|
||||
if (FirmwareVersion != null)
|
||||
{
|
||||
if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
|
||||
{
|
||||
// 飞控提供了 canTakeOff。
|
||||
IsGpsAccurate = canTakeOff;
|
||||
}
|
||||
else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
|
||||
{
|
||||
// 2.0 飞行器，飞控未提供 canTakeOff。
|
||||
IsGpsAccurate = SatCount >= 12;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 1.0 飞行器，飞控不提供 canTakeOff。
|
||||
IsGpsAccurate = SatCount >= 6;
|
||||
}
|
||||
}
|
||||
else
|
||||
IsGpsAccurate = SatCount >= 8;
|
||||
|
||||
FlightDistance2D = _internalCopter.FlightDistance2D;
|
||||
|
||||
RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
|
||||
}
|
||||
|
||||
private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
|
||||
{
|
||||
switch (e.Packet[5])
|
||||
{
|
||||
case MAVLINK_MSG_ID_MISSION_COUNT:
|
||||
MissionCount = _internalCopter.WpCount;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
|
||||
{
|
||||
switch (e.Packet[5])
|
||||
{
|
||||
case MAVLINK_MSG_ID_MISSION_ACK:
|
||||
AnalyzeMissionAckPacket(e.Packet);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_MISSION_COUNT:
|
||||
AnalyzeMissionCountPacket(e.Packet);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_MISSION_ITEM:
|
||||
AnalyzeMissionItemPacket(e.Packet);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_MISSION_REQUEST:
|
||||
AnalyzeMissionRequestPacket(e.Packet);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_SET_PAIR:
|
||||
AnalyzeSetPairPacket(e.Packet);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
#if PRIVATE
|
||||
protected virtual void RegisterInternalCopterEventHandlers()
|
||||
#else
|
||||
|
||||
private void RegisterInternalCopterEventHandlers()
|
||||
#endif
|
||||
{
|
||||
_internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
|
||||
_internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
|
||||
_internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
|
||||
_internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
|
||||
_internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
|
||||
_internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
|
||||
_internalCopter.PacketHandled += _internalCopter_PacketHandled;
|
||||
_internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
|
||||
_internalCopter.MessageCreated += _internalCopter_MessageCreated;
|
||||
}
|
||||
|
||||
private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e)
|
||||
{
|
||||
var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message };
|
||||
RaiseTextReceived(txte);
|
||||
}
|
||||
|
||||
#region 计算剩余电量
|
||||
|
||||
private int bPerTimes;
|
||||
private int outBatteryPer;
|
||||
private int[] tPerTimes = new int[20];
|
||||
private int v_num;
|
||||
|
||||
/// <summary>
|
||||
/// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
|
||||
/// </summary>
|
||||
private void CalcBatteryPer()
|
||||
{
|
||||
float volmax = 0f;
|
||||
float volmin = 0f;
|
||||
if (Voltage > 5 && Voltage < 9)
|
||||
{
|
||||
volmax = 8.2f;
|
||||
volmin = 7f;
|
||||
}
|
||||
else if (Voltage >= 9
|
||||
&& Voltage < 13.6)
|
||||
{
|
||||
volmax = 11.6f;
|
||||
volmin = 10.2f;
|
||||
}
|
||||
else if (Voltage >= 13.6
|
||||
&& Voltage < 17.2)
|
||||
{
|
||||
volmax = 16.3f;
|
||||
volmin = 14.2f;
|
||||
}
|
||||
else if (Voltage >= 17.2
|
||||
&& Voltage < 26.2)
|
||||
{
|
||||
volmax = 24.8f;
|
||||
volmin = 21.2f;
|
||||
}
|
||||
|
||||
int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));
|
||||
|
||||
if (batteryPer == -1 || volmax == 0 || volmin == 0)
|
||||
return;
|
||||
|
||||
if (bPerTimes < 20)
|
||||
{
|
||||
tPerTimes[bPerTimes] = batteryPer;
|
||||
bPerTimes += 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
tPerTimes[v_num] = batteryPer;
|
||||
v_num++;
|
||||
if (v_num == 20)
|
||||
v_num = 0;
|
||||
}
|
||||
for (int i = 0; i < bPerTimes; i++)
|
||||
{
|
||||
outBatteryPer += tPerTimes[i];
|
||||
}
|
||||
|
||||
outBatteryPer = outBatteryPer / bPerTimes;
|
||||
if (outBatteryPer < BatteryPer && bPerTimes > 18)
|
||||
{
|
||||
BatteryPer = (byte)outBatteryPer;
|
||||
}
|
||||
}
|
||||
|
||||
#endregion 计算剩余电量
|
||||
}
|
||||
}
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using static Plane.Protocols.MAVLink;
|
||||
|
||||
namespace Plane.Copters
|
||||
{
|
||||
|
||||
|
||||
public partial class PLCopter : CopterImplSharedPart
|
||||
{
|
||||
|
||||
private bool _fetchingFirmwareVersion;
|
||||
|
||||
private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
|
||||
{
|
||||
IsConnected = _internalCopter.IsConnected;
|
||||
}
|
||||
|
||||
private void _internalCopter_GetLogDataEvent(string log)
|
||||
{
|
||||
//飞机消息日志，后面需要改为记录方式
|
||||
StatusText =Name+":"+log;
|
||||
var e = new MessageCreatedEventArgs { Message = StatusText };
|
||||
RaiseTextReceived(e);
|
||||
}
|
||||
|
||||
|
||||
private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
|
||||
{
|
||||
switch (StreamType)
|
||||
{
|
||||
case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
|
||||
{
|
||||
SatCount = _internalCopter.satcount;
|
||||
GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
|
||||
GpsHdop = _internalCopter.gpshdop;
|
||||
CopterErrorCode =_internalCopter.errorcode;
|
||||
CopterPreCheckPass = _internalCopter.precheckok;
|
||||
CopterErrorString = getcoptererrorstr(_internalCopter.errorcode);
|
||||
Altitude = _internalCopter.gpsalt;
|
||||
Retain = BitConverter.GetBytes(_internalCopter.retain);
|
||||
|
||||
Voltage = _internalCopter.battery_voltage;
|
||||
CommModuleMode = (FlightMode)_internalCopter.commModuleMode;
|
||||
CommModuleVersion = _internalCopter.commModuleVersion;
|
||||
IsUnlocked = _internalCopter.isUnlocked;
|
||||
//Yaw = _internalCopter.yaw;
|
||||
Heading = _internalCopter.heading;
|
||||
HeartbeatCount++;
|
||||
|
||||
if (SatCount > 8)
|
||||
{
|
||||
IsGpsAccurate = true;
|
||||
RecordLat = _internalCopter.lat;
|
||||
RecordLng = _internalCopter.lng;
|
||||
}
|
||||
|
||||
Latitude = RecordLat;
|
||||
Longitude = RecordLng;
|
||||
UpdateFlightDataIfNeeded();
|
||||
RaiseLocationChangedIfNeeded();
|
||||
|
||||
RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
|
||||
|
||||
break;
|
||||
}
|
||||
case MAVLINK_MSG_ID_ATTITUDE: //无用
|
||||
{
|
||||
Roll = _internalCopter.roll;
|
||||
Pitch = _internalCopter.pitch;
|
||||
Yaw = _internalCopter.yaw;
|
||||
TimebootMs = _internalCopter.timebootms;
|
||||
RaiseAttitudeChanged();
|
||||
|
||||
break;
|
||||
}
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要
|
||||
{
|
||||
Channel1 = _internalCopter.ch1in;
|
||||
Channel2 = _internalCopter.ch2in;
|
||||
Channel3 = _internalCopter.ch3in;
|
||||
Channel4 = _internalCopter.ch4in;
|
||||
Channel5 = _internalCopter.ch5in;
|
||||
Channel6 = _internalCopter.ch6in;
|
||||
Channel7 = _internalCopter.ch7in;
|
||||
Channel8 = _internalCopter.ch8in;
|
||||
|
||||
RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);
|
||||
|
||||
break;
|
||||
}
|
||||
case MAVLINK_MSG_ID_VFR_HUD:
|
||||
{
|
||||
Heading = _internalCopter.heading; //有用
|
||||
Altitude = _internalCopter.alt; //有用
|
||||
AirSpeed = _internalCopter.airspeed; //没用
|
||||
GroundSpeed = _internalCopter.groundspeed; //没用
|
||||
|
||||
RaiseAltitudeChangedIfNeeded();
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
|
||||
{
|
||||
IsUnlocked = _internalCopter.armed;
|
||||
Mode = (FlightMode)_internalCopter.mode;
|
||||
// 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
|
||||
HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
|
||||
++HeartbeatCount;
|
||||
|
||||
IsCheckingConnection = false;
|
||||
/*
|
||||
if (FirmwareVersion == null && !_fetchingFirmwareVersion)
|
||||
{
|
||||
try
|
||||
{
|
||||
_fetchingFirmwareVersion = true;
|
||||
FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
|
||||
}
|
||||
catch (TimeoutException)
|
||||
{
|
||||
// 吞掉。
|
||||
}
|
||||
finally
|
||||
{
|
||||
_fetchingFirmwareVersion = false;
|
||||
}
|
||||
}
|
||||
*/
|
||||
RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
|
||||
}
|
||||
|
||||
private void _internalCopter_ReceiveSensorEvent(object sender)
|
||||
{
|
||||
RaiseSensorDataReceived(EventArgs.Empty);
|
||||
}
|
||||
|
||||
private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff)
|
||||
{
|
||||
Voltage = _internalCopter.battery_voltage / 1000;
|
||||
|
||||
if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。
|
||||
{
|
||||
CalcBatteryPer();
|
||||
}
|
||||
else
|
||||
{
|
||||
BatteryPer = _internalCopter.battery_remaining;
|
||||
}
|
||||
|
||||
const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308;
|
||||
const int MIN_VERSION_OF_GENERATION2 = 0x3000;
|
||||
if (FirmwareVersion != null)
|
||||
{
|
||||
if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
|
||||
{
|
||||
// 飞控提供了 canTakeOff。
|
||||
IsGpsAccurate = canTakeOff;
|
||||
}
|
||||
else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
|
||||
{
|
||||
// 2.0 飞行器，飞控未提供 canTakeOff。
|
||||
IsGpsAccurate = SatCount >= 12;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 1.0 飞行器，飞控不提供 canTakeOff。
|
||||
IsGpsAccurate = SatCount >= 6;
|
||||
}
|
||||
}
|
||||
else
|
||||
IsGpsAccurate = SatCount >= 8;
|
||||
|
||||
FlightDistance2D = _internalCopter.FlightDistance2D;
|
||||
|
||||
RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
|
||||
}
|
||||
|
||||
private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
|
||||
{
|
||||
switch (e.Packet[5])
|
||||
{
|
||||
case MAVLINK_MSG_ID_MISSION_COUNT:
|
||||
MissionCount = _internalCopter.WpCount;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
|
||||
{
|
||||
switch (e.Packet[5])
|
||||
{
|
||||
case MAVLINK_MSG_ID_MISSION_ACK:
|
||||
AnalyzeMissionAckPacket(e.Packet);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_MISSION_COUNT:
|
||||
AnalyzeMissionCountPacket(e.Packet);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_MISSION_ITEM:
|
||||
AnalyzeMissionItemPacket(e.Packet);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_MISSION_REQUEST:
|
||||
AnalyzeMissionRequestPacket(e.Packet);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_SET_PAIR:
|
||||
AnalyzeSetPairPacket(e.Packet);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
#if PRIVATE
|
||||
protected virtual void RegisterInternalCopterEventHandlers()
|
||||
#else
|
||||
|
||||
private void RegisterInternalCopterEventHandlers()
|
||||
#endif
|
||||
{
|
||||
_internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
|
||||
_internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
|
||||
_internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
|
||||
_internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
|
||||
_internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
|
||||
_internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
|
||||
_internalCopter.PacketHandled += _internalCopter_PacketHandled;
|
||||
_internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
|
||||
_internalCopter.MessageCreated += _internalCopter_MessageCreated;
|
||||
}
|
||||
|
||||
private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e)
|
||||
{
|
||||
var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message };
|
||||
RaiseTextReceived(txte);
|
||||
}
|
||||
|
||||
#region 计算剩余电量
|
||||
|
||||
private int bPerTimes;
|
||||
private int outBatteryPer;
|
||||
private int[] tPerTimes = new int[20];
|
||||
private int v_num;
|
||||
|
||||
/// <summary>
|
||||
/// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
|
||||
/// </summary>
|
||||
private void CalcBatteryPer()
|
||||
{
|
||||
float volmax = 0f;
|
||||
float volmin = 0f;
|
||||
if (Voltage > 5 && Voltage < 9)
|
||||
{
|
||||
volmax = 8.2f;
|
||||
volmin = 7f;
|
||||
}
|
||||
else if (Voltage >= 9
|
||||
&& Voltage < 13.6)
|
||||
{
|
||||
volmax = 11.6f;
|
||||
volmin = 10.2f;
|
||||
}
|
||||
else if (Voltage >= 13.6
|
||||
&& Voltage < 17.2)
|
||||
{
|
||||
volmax = 16.3f;
|
||||
volmin = 14.2f;
|
||||
}
|
||||
else if (Voltage >= 17.2
|
||||
&& Voltage < 26.2)
|
||||
{
|
||||
volmax = 24.8f;
|
||||
volmin = 21.2f;
|
||||
}
|
||||
|
||||
int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));
|
||||
|
||||
if (batteryPer == -1 || volmax == 0 || volmin == 0)
|
||||
return;
|
||||
|
||||
if (bPerTimes < 20)
|
||||
{
|
||||
tPerTimes[bPerTimes] = batteryPer;
|
||||
bPerTimes += 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
tPerTimes[v_num] = batteryPer;
|
||||
v_num++;
|
||||
if (v_num == 20)
|
||||
v_num = 0;
|
||||
}
|
||||
for (int i = 0; i < bPerTimes; i++)
|
||||
{
|
||||
outBatteryPer += tPerTimes[i];
|
||||
}
|
||||
|
||||
outBatteryPer = outBatteryPer / bPerTimes;
|
||||
if (outBatteryPer < BatteryPer && bPerTimes > 18)
|
||||
{
|
||||
BatteryPer = (byte)outBatteryPer;
|
||||
}
|
||||
}
|
||||
|
||||
#endregion 计算剩余电量
|
||||
}
|
||||
}
|
||||
|
||||
@ -144,7 +144,7 @@ namespace Plane.Copters
|
||||
return Task.FromResult(true);
|
||||
}
|
||||
|
||||
protected override Task FlyToCoreAsync(double lat, double lng, float alt)
|
||||
protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
|
||||
{
|
||||
return TaskUtils.CompletedTask;
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,84 +1,86 @@
|
||||
using System;
|
||||
|
||||
namespace Plane.Copters
|
||||
{
|
||||
#if PRIVATE
|
||||
public
|
||||
#else
|
||||
internal
|
||||
#endif
|
||||
enum FlightMode
|
||||
{
|
||||
// 林俊清，20150608：不可将以下枚举项重命名。
|
||||
|
||||
STABILIZE = 0, // hold level position
|
||||
|
||||
ACRO = 1, // rate control // 林俊清, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/
|
||||
|
||||
ALT_HOLD = 2, // AUTO control
|
||||
|
||||
AUTO = 3, // AUTO control
|
||||
|
||||
GUIDED = 4, // AUTO control
|
||||
|
||||
LOITER = 5, // Hold a single location
|
||||
|
||||
RTL = 6, // AUTO control
|
||||
|
||||
CIRCLE = 7,
|
||||
|
||||
POSITION = 8, // 林俊清, 20160205, 位置模式：http://copter.ardupilot.cn/wiki/POSITION-mode/
|
||||
|
||||
LAND = 9, // AUTO control
|
||||
|
||||
OF_LOITER = 10, // 林俊清, 20160205, 光流悬停模式：http://copter.ardupilot.cn/wiki/loiter-mode/ 底部。
|
||||
|
||||
TOY = 11
|
||||
}
|
||||
|
||||
internal static class FightModeExtensions
|
||||
{
|
||||
public static string GetModeString(this FlightMode flightMode)
|
||||
{
|
||||
switch (flightMode)
|
||||
{
|
||||
case FlightMode.ALT_HOLD:
|
||||
return "ALT HOLD";
|
||||
|
||||
case FlightMode.POSITION:
|
||||
return "POS HOLD";
|
||||
|
||||
default:
|
||||
return Enum.GetName(typeof(FlightMode), flightMode);
|
||||
}
|
||||
}
|
||||
|
||||
public static bool NeedGps(this FlightMode flightMode)
|
||||
{
|
||||
switch (flightMode)
|
||||
{
|
||||
case FlightMode.AUTO:
|
||||
case FlightMode.GUIDED:
|
||||
case FlightMode.LOITER:
|
||||
case FlightMode.RTL:
|
||||
case FlightMode.CIRCLE:
|
||||
case FlightMode.LAND:
|
||||
case FlightMode.POSITION:
|
||||
default:
|
||||
return true;
|
||||
|
||||
case FlightMode.STABILIZE:
|
||||
case FlightMode.ACRO:
|
||||
case FlightMode.ALT_HOLD:
|
||||
case FlightMode.OF_LOITER:
|
||||
case FlightMode.TOY:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode)
|
||||
{
|
||||
return (PlaneCopter.ac2modes)flightMode;
|
||||
}
|
||||
}
|
||||
}
|
||||
using System;
|
||||
|
||||
namespace Plane.Copters
|
||||
{
|
||||
#if PRIVATE
|
||||
public
|
||||
#else
|
||||
internal
|
||||
#endif
|
||||
enum FlightMode
|
||||
{
|
||||
// 王海，20150608：不可将以下枚举项重命名。
|
||||
|
||||
STABILIZE = 0, // hold level position
|
||||
|
||||
ACRO = 1, // rate control // 王海, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/
|
||||
|
||||
ALT_HOLD = 2, // AUTO control
|
||||
|
||||
AUTO = 3, // AUTO control
|
||||
|
||||
GUIDED = 4, // AUTO control
|
||||
|
||||
LOITER = 5, // Hold a single location
|
||||
|
||||
RTL = 6, // AUTO control
|
||||
|
||||
CIRCLE = 7,
|
||||
|
||||
POSITION = 8, // 王海, 20160205, 位置模式：http://copter.ardupilot.cn/wiki/POSITION-mode/
|
||||
|
||||
LAND = 9, // AUTO control
|
||||
|
||||
OF_LOITER = 10, // 王海, 20160205, 光流悬停模式：http://copter.ardupilot.cn/wiki/loiter-mode/ 底部。
|
||||
|
||||
TOY = 11,
|
||||
|
||||
EMERG_RTL=12, //紧急返航模式
|
||||
}
|
||||
|
||||
internal static class FightModeExtensions
|
||||
{
|
||||
public static string GetModeString(this FlightMode flightMode)
|
||||
{
|
||||
switch (flightMode)
|
||||
{
|
||||
case FlightMode.ALT_HOLD:
|
||||
return "ALT HOLD";
|
||||
|
||||
case FlightMode.POSITION:
|
||||
return "POS HOLD";
|
||||
|
||||
default:
|
||||
return Enum.GetName(typeof(FlightMode), flightMode);
|
||||
}
|
||||
}
|
||||
|
||||
public static bool NeedGps(this FlightMode flightMode)
|
||||
{
|
||||
switch (flightMode)
|
||||
{
|
||||
case FlightMode.AUTO:
|
||||
case FlightMode.GUIDED:
|
||||
case FlightMode.LOITER:
|
||||
case FlightMode.RTL:
|
||||
case FlightMode.CIRCLE:
|
||||
case FlightMode.LAND:
|
||||
case FlightMode.POSITION:
|
||||
default:
|
||||
return true;
|
||||
|
||||
case FlightMode.STABILIZE:
|
||||
case FlightMode.ACRO:
|
||||
case FlightMode.ALT_HOLD:
|
||||
case FlightMode.OF_LOITER:
|
||||
case FlightMode.TOY:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode)
|
||||
{
|
||||
return (PlaneCopter.ac2modes)flightMode;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -292,7 +292,7 @@ namespace Plane.Copters
|
||||
public async Task TakeOffAsync(float alt)
|
||||
{
|
||||
var currentTakeOffCount = ++_takeOffCount;
|
||||
// 林俊清, 20160312, 从固件版本 3.1.1（0x3101）开始支持直接解锁起飞命令。
|
||||
// 王海, 20160312, 从固件版本 3.1.1（0x3101）开始支持直接解锁起飞命令。
|
||||
//if (FirmwareVersion >= 0x3101)
|
||||
{
|
||||
await SetChannelsAsync(
|
||||
@ -349,7 +349,7 @@ namespace Plane.Copters
|
||||
ch4: 1500
|
||||
).ConfigureAwait(false);
|
||||
|
||||
if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false))
|
||||
if (await SetModeAsync(FlightMode.LOITER).ConfigureAwait(false))
|
||||
{
|
||||
await _internalCopter.DoARMAsync(true).ConfigureAwait(false);
|
||||
}
|
||||
@ -418,7 +418,7 @@ namespace Plane.Copters
|
||||
return !anotherSetModeActionCalled && Mode == mode;
|
||||
}
|
||||
|
||||
protected override async Task FlyToCoreAsync(double lat, double lng, float alt)
|
||||
protected override async Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
|
||||
{
|
||||
if (!IsEmergencyHoverActive)
|
||||
{
|
||||
|
||||
@ -46,7 +46,7 @@ namespace Plane.Copters
|
||||
((ICollection)bitArray).CopyTo(array, 0);
|
||||
return array[0];
|
||||
}
|
||||
// 林俊清，20150703：BitArray 的构造函数的参数是数组长度，这里根本不对啊。
|
||||
// 王海，20150703：BitArray 的构造函数的参数是数组长度，这里根本不对啊。
|
||||
//set
|
||||
//{
|
||||
// bitArray = new BitArray(value);
|
||||
|
||||
@ -1,402 +1,404 @@
|
||||
//#define LOG_PACKETS
|
||||
|
||||
using Plane.Protocols;
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using TaskTools.Utilities;
|
||||
|
||||
#if LOG_PACKETS
|
||||
|
||||
using System.Diagnostics;
|
||||
|
||||
#endif
|
||||
|
||||
namespace Plane.Copters
|
||||
{
|
||||
partial class PlaneCopter
|
||||
{
|
||||
private void AnalyzeAttitudePacket(byte[] buffer)
|
||||
{
|
||||
var att = buffer.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
|
||||
roll = att.roll * rad2deg;
|
||||
pitch = att.pitch * rad2deg;
|
||||
yaw = att.yaw * rad2deg;
|
||||
timebootms = att.time_boot_ms;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeGlobalPositionIntPacket(byte[] buffer)
|
||||
{
|
||||
var loc = buffer.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
|
||||
useLocation = true;
|
||||
if (loc.lat == 0 && loc.lon == 0)
|
||||
{
|
||||
useLocation = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
gpsalt = loc.alt / 1000.0f;
|
||||
lat = loc.lat / 10000000.0;
|
||||
lng = loc.lon / 10000000.0;
|
||||
}
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
#if LOG_GPS_RAW_INT
|
||||
private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch();
|
||||
private TimeSpan _lastGpsRawIntPacketTimeSpan;
|
||||
#endif
|
||||
|
||||
private void AnalyzeGpsRawIntPacket(byte[] buffer)
|
||||
{
|
||||
#if LOG_GPS_RAW_INT
|
||||
if (!_lastGpsRawIntPacketTime.IsRunning)
|
||||
{
|
||||
_lastGpsRawIntPacketTime.Start();
|
||||
}
|
||||
else
|
||||
{
|
||||
var elapsed = _lastGpsRawIntPacketTime.Elapsed;
|
||||
Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt");
|
||||
_lastGpsRawIntPacketTimeSpan = elapsed;
|
||||
}
|
||||
#endif
|
||||
var gps = buffer.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
|
||||
long mylat = BitConverter.ToInt32(buffer, 14);
|
||||
long mylong = BitConverter.ToInt32(buffer, 18);
|
||||
//if (!useLocation)
|
||||
//{
|
||||
lat = gps.lat * 1.0e-7;
|
||||
lng = gps.lon * 1.0e-7;
|
||||
//}
|
||||
gpsalt = gps.alt * 1.0e-7f; // using vfr as includes baro calc
|
||||
gpsstatus = gps.fix_type; //0-1: no fix, 2: 2D fix, 3: 3D fix
|
||||
gpshdop = (float)Math.Round(gps.eph / 100.0, 2);
|
||||
satcount = gps.satellites_visible;
|
||||
groundspeed = gps.vel * 1.0e-2f;
|
||||
groundcourse = gps.cog * 1.0e-2f;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
#if LOG_PACKETS
|
||||
private DateTime _lastHeartbeatTime = DateTime.MinValue;
|
||||
#endif
|
||||
|
||||
private void AnalyzeHeartbeatPacket(byte[] buffer)
|
||||
{
|
||||
#if LOG_PACKETS
|
||||
var now = DateTime.Now;
|
||||
if (_lastHeartbeatTime != DateTime.MinValue)
|
||||
{
|
||||
Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔");
|
||||
}
|
||||
_lastHeartbeatTime = now;
|
||||
#endif
|
||||
DataTimeOut = 0;
|
||||
var hb = buffer.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
|
||||
mavlinkversion = hb.mavlink_version;
|
||||
aptype = (MAVLink.MAV_TYPE)hb.type;
|
||||
apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot;
|
||||
sysid = buffer[3];
|
||||
compid = buffer[4];
|
||||
recvpacketcount = buffer[2];
|
||||
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
|
||||
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
|
||||
mode = hb.custom_mode;
|
||||
|
||||
var handler = ReceiveHeartBearEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() =>
|
||||
{
|
||||
handler(this, iErrorData, iDataCount);
|
||||
});
|
||||
}
|
||||
|
||||
if (_autoRequestData && mavlinkversion == 3 && SendReq == false)
|
||||
{
|
||||
SendReq = true;
|
||||
Task.Run(GetCopterDataAsync);
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeMemInfoPacket(byte[] buffer)
|
||||
{
|
||||
var mem = buffer.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
|
||||
freemem = mem.freemem;
|
||||
brklevel = mem.brkval;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeMissionCurrentPacket(byte[] buffer)
|
||||
{
|
||||
var wpcur = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeNavControllerOutputPacket(byte[] buffer)
|
||||
{
|
||||
var nav = buffer.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
|
||||
nav_roll = nav.nav_roll;
|
||||
nav_pitch = nav.nav_pitch;
|
||||
nav_bearing = nav.nav_bearing;
|
||||
target_bearing = nav.target_bearing;
|
||||
wp_dist = nav.wp_dist;
|
||||
alt_error = nav.alt_error;
|
||||
aspd_error = nav.aspd_error / 100.0f;
|
||||
xtrack_error = nav.xtrack_error;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeParamValuePacket(byte[] buffer)
|
||||
{
|
||||
var par = buffer.ByteArrayToStructure<MAVLink.mavlink_param_value_t>(6);
|
||||
param_total = (par.param_count);
|
||||
string paramID = Encoding.ASCII.GetString(par.param_id);
|
||||
int pos = paramID.IndexOf('\0');
|
||||
if (pos != -1)
|
||||
{
|
||||
paramID = paramID.Substring(0, pos);
|
||||
}
|
||||
try
|
||||
{
|
||||
param[paramID] = (par.param_value);
|
||||
param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type;
|
||||
}
|
||||
catch (IndexOutOfRangeException ex)
|
||||
{
|
||||
Debug.WriteLine(ex);
|
||||
}
|
||||
|
||||
var handler = ReceiveParamEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() =>
|
||||
{
|
||||
handler(this, paramID, par.param_index, par.param_count);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeRadioPacket(byte[] buffer)
|
||||
{
|
||||
var radio = buffer.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
|
||||
//Debug.Print("fix:" + radio.fixedp + " noise:" + radio.noise + " remnoise:" + radio.remnoise + " remrssi:" + radio.remrssi + " rssi:" + radio.rssi + " rxerr:" + radio.rxerrors + " txbuf:" + radio.txbuf);
|
||||
remrssi = radio.remrssi;
|
||||
rssi = radio.rssi;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeRawImuPacket(byte[] buffer)
|
||||
{
|
||||
var imu = buffer.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
|
||||
|
||||
gx = imu.xgyro;
|
||||
gy = imu.ygyro;
|
||||
gz = imu.zgyro;
|
||||
|
||||
ax = imu.xacc;
|
||||
ay = imu.yacc;
|
||||
az = imu.zacc;
|
||||
|
||||
mx = imu.xmag;
|
||||
my = imu.ymag;
|
||||
mz = imu.zmag;
|
||||
|
||||
var handler = ReceiveSensorEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() =>
|
||||
{
|
||||
handler(this);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeRCChannelsRawPacket(byte[] buffer)
|
||||
{
|
||||
var rcin = buffer.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
|
||||
|
||||
ch1in = rcin.chan1_raw;
|
||||
ch2in = rcin.chan2_raw;
|
||||
ch3in = rcin.chan3_raw;
|
||||
ch4in = rcin.chan4_raw;
|
||||
ch5in = rcin.chan5_raw;
|
||||
ch6in = rcin.chan6_raw;
|
||||
ch7in = rcin.chan7_raw;
|
||||
ch8in = rcin.chan8_raw;
|
||||
if (bInitChannel == false)
|
||||
{
|
||||
bInitChannel = true;
|
||||
}
|
||||
|
||||
//percent
|
||||
rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
|
||||
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeScaledImu2Packet(byte[] buffer)
|
||||
{
|
||||
var imu2 = buffer.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
|
||||
gx2 = imu2.xgyro;
|
||||
gy2 = imu2.ygyro;
|
||||
gz2 = imu2.zgyro;
|
||||
|
||||
ax2 = imu2.xacc;
|
||||
ay2 = imu2.yacc;
|
||||
az2 = imu2.zacc;
|
||||
|
||||
mx2 = imu2.xmag;
|
||||
my2 = imu2.ymag;
|
||||
mz2 = imu2.zmag;
|
||||
|
||||
var handler = ReceiveScaleImu2Event;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() => handler(this));
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeSensorOffsetsPacket(byte[] buffer)
|
||||
{
|
||||
var sensofs = buffer.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
|
||||
|
||||
mag_ofs_x = sensofs.mag_ofs_x;
|
||||
mag_ofs_y = sensofs.mag_ofs_y;
|
||||
mag_ofs_z = sensofs.mag_ofs_z;
|
||||
mag_declination = sensofs.mag_declination;
|
||||
|
||||
raw_press = sensofs.raw_press;
|
||||
raw_temp = sensofs.raw_temp;
|
||||
|
||||
gyro_cal_x = sensofs.gyro_cal_x;
|
||||
gyro_cal_y = sensofs.gyro_cal_y;
|
||||
gyro_cal_z = sensofs.gyro_cal_z;
|
||||
|
||||
accel_cal_x = sensofs.accel_cal_x;
|
||||
accel_cal_y = sensofs.accel_cal_y;
|
||||
accel_cal_z = sensofs.accel_cal_z;
|
||||
|
||||
var handler = ReceiveSensorEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() =>
|
||||
{
|
||||
handler(this);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeStatusTextPacket(byte[] buffer)
|
||||
{
|
||||
var stext = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
|
||||
var length = Array.IndexOf<byte>(stext.text, 0);
|
||||
var log = Encoding.ASCII.GetString(stext.text, 0, length);
|
||||
var handler = GetLogDataEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() => handler(log));
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeSysStatusPacket(byte[] buffer)
|
||||
{
|
||||
var sysstatus = buffer.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
|
||||
battery_voltage = sysstatus.voltage_battery;
|
||||
battery_remaining = sysstatus.battery_remaining;
|
||||
current_battery = sysstatus.current_battery / 100.0f;
|
||||
|
||||
var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
|
||||
var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
|
||||
|
||||
if (sensors_health.gps != sensors_enabled.gps)
|
||||
{
|
||||
bGPSBadHealth = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
bGPSBadHealth = false;
|
||||
}
|
||||
if (sensors_health.gyro != sensors_enabled.gyro)
|
||||
{
|
||||
bGyroBadHealth = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
bGyroBadHealth = false;
|
||||
}
|
||||
if (sensors_health.accelerometer != sensors_enabled.accelerometer)
|
||||
{
|
||||
bAccel = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
bAccel = false;
|
||||
}
|
||||
if (sensors_health.compass != sensors_enabled.compass)
|
||||
{
|
||||
bCompass = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
bCompass = false;
|
||||
}
|
||||
if (sensors_health.barometer != sensors_enabled.barometer)
|
||||
{
|
||||
bBarometer = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
bBarometer = false;
|
||||
}
|
||||
FlightDistance2D = sysstatus.errors_count2;
|
||||
var handler = ReceiveSysStatusEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() =>
|
||||
{
|
||||
handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeVfrHudPacket(byte[] buffer)
|
||||
{
|
||||
var vfr = buffer.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
|
||||
groundspeed = vfr.groundspeed;
|
||||
airspeed = vfr.airspeed;
|
||||
altorigin = vfr.alt; // this might include baro
|
||||
ch3percent = vfr.throttle;
|
||||
heading = vfr.heading;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 处理通信模块发过来的数据
|
||||
/// </summary>
|
||||
/// <param name="buffer"></param>
|
||||
public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version)
|
||||
{
|
||||
var info = buffer.ByteArrayToStructure<MavComm.comm_copter_info>(0);
|
||||
lat = info.lat * 1.0e-7;
|
||||
lng = info.lng * 1.0e-7;
|
||||
commModuleMode = (uint)info.control_mode;
|
||||
gpsstatus = (byte)info.gps_status;
|
||||
gpsalt = ((float)info.alt) / 1000;
|
||||
satcount = info.gps_num_sats;
|
||||
isUnlocked = info.lock_status == 1;
|
||||
|
||||
battery_voltage = ((float)info.battery_voltage) / 1000;
|
||||
|
||||
heading = (short)((info.heading / 100) % 360);
|
||||
|
||||
commModuleVersion = version;
|
||||
|
||||
retain = info.retain;
|
||||
RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT);
|
||||
}
|
||||
}
|
||||
}
|
||||
//#define LOG_PACKETS
|
||||
|
||||
using Plane.Protocols;
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using TaskTools.Utilities;
|
||||
|
||||
#if LOG_PACKETS
|
||||
|
||||
using System.Diagnostics;
|
||||
|
||||
#endif
|
||||
|
||||
namespace Plane.Copters
|
||||
{
|
||||
partial class PlaneCopter
|
||||
{
|
||||
private void AnalyzeAttitudePacket(byte[] buffer)
|
||||
{
|
||||
var att = buffer.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
|
||||
roll = att.roll * rad2deg;
|
||||
pitch = att.pitch * rad2deg;
|
||||
yaw = att.yaw * rad2deg;
|
||||
timebootms = att.time_boot_ms;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeGlobalPositionIntPacket(byte[] buffer)
|
||||
{
|
||||
var loc = buffer.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
|
||||
useLocation = true;
|
||||
if (loc.lat == 0 && loc.lon == 0)
|
||||
{
|
||||
useLocation = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
gpsalt = loc.alt / 1000.0f;
|
||||
lat = loc.lat / 10000000.0;
|
||||
lng = loc.lon / 10000000.0;
|
||||
}
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
#if LOG_GPS_RAW_INT
|
||||
private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch();
|
||||
private TimeSpan _lastGpsRawIntPacketTimeSpan;
|
||||
#endif
|
||||
|
||||
private void AnalyzeGpsRawIntPacket(byte[] buffer)
|
||||
{
|
||||
#if LOG_GPS_RAW_INT
|
||||
if (!_lastGpsRawIntPacketTime.IsRunning)
|
||||
{
|
||||
_lastGpsRawIntPacketTime.Start();
|
||||
}
|
||||
else
|
||||
{
|
||||
var elapsed = _lastGpsRawIntPacketTime.Elapsed;
|
||||
Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt");
|
||||
_lastGpsRawIntPacketTimeSpan = elapsed;
|
||||
}
|
||||
#endif
|
||||
var gps = buffer.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
|
||||
long mylat = BitConverter.ToInt32(buffer, 14);
|
||||
long mylong = BitConverter.ToInt32(buffer, 18);
|
||||
//if (!useLocation)
|
||||
//{
|
||||
lat = gps.lat * 1.0e-7;
|
||||
lng = gps.lon * 1.0e-7;
|
||||
//}
|
||||
gpsalt = gps.alt * 1.0e-7f; // using vfr as includes baro calc
|
||||
gpsstatus = gps.fix_type; //0-1: no fix, 2: 2D fix, 3: 3D fix
|
||||
gpshdop = (float)Math.Round(gps.eph / 100.0, 2);
|
||||
satcount = gps.satellites_visible;
|
||||
groundspeed = gps.vel * 1.0e-2f;
|
||||
groundcourse = gps.cog * 1.0e-2f;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
#if LOG_PACKETS
|
||||
private DateTime _lastHeartbeatTime = DateTime.MinValue;
|
||||
#endif
|
||||
|
||||
private void AnalyzeHeartbeatPacket(byte[] buffer)
|
||||
{
|
||||
#if LOG_PACKETS
|
||||
var now = DateTime.Now;
|
||||
if (_lastHeartbeatTime != DateTime.MinValue)
|
||||
{
|
||||
Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔");
|
||||
}
|
||||
_lastHeartbeatTime = now;
|
||||
#endif
|
||||
DataTimeOut = 0;
|
||||
var hb = buffer.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
|
||||
mavlinkversion = hb.mavlink_version;
|
||||
aptype = (MAVLink.MAV_TYPE)hb.type;
|
||||
apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot;
|
||||
sysid = buffer[3];
|
||||
compid = buffer[4];
|
||||
recvpacketcount = buffer[2];
|
||||
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
|
||||
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
|
||||
mode = hb.custom_mode;
|
||||
|
||||
var handler = ReceiveHeartBearEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() =>
|
||||
{
|
||||
handler(this, iErrorData, iDataCount);
|
||||
});
|
||||
}
|
||||
|
||||
if (_autoRequestData && mavlinkversion == 3 && SendReq == false)
|
||||
{
|
||||
SendReq = true;
|
||||
Task.Run(GetCopterDataAsync);
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeMemInfoPacket(byte[] buffer)
|
||||
{
|
||||
var mem = buffer.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
|
||||
freemem = mem.freemem;
|
||||
brklevel = mem.brkval;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeMissionCurrentPacket(byte[] buffer)
|
||||
{
|
||||
var wpcur = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeNavControllerOutputPacket(byte[] buffer)
|
||||
{
|
||||
var nav = buffer.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
|
||||
nav_roll = nav.nav_roll;
|
||||
nav_pitch = nav.nav_pitch;
|
||||
nav_bearing = nav.nav_bearing;
|
||||
target_bearing = nav.target_bearing;
|
||||
wp_dist = nav.wp_dist;
|
||||
alt_error = nav.alt_error;
|
||||
aspd_error = nav.aspd_error / 100.0f;
|
||||
xtrack_error = nav.xtrack_error;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeParamValuePacket(byte[] buffer)
|
||||
{
|
||||
var par = buffer.ByteArrayToStructure<MAVLink.mavlink_param_value_t>(6);
|
||||
param_total = (par.param_count);
|
||||
string paramID = Encoding.ASCII.GetString(par.param_id);
|
||||
int pos = paramID.IndexOf('\0');
|
||||
if (pos != -1)
|
||||
{
|
||||
paramID = paramID.Substring(0, pos);
|
||||
}
|
||||
try
|
||||
{
|
||||
param[paramID] = (par.param_value);
|
||||
param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type;
|
||||
}
|
||||
catch (IndexOutOfRangeException ex)
|
||||
{
|
||||
Debug.WriteLine(ex);
|
||||
}
|
||||
|
||||
var handler = ReceiveParamEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() =>
|
||||
{
|
||||
handler(this, paramID, par.param_index, par.param_count);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeRadioPacket(byte[] buffer)
|
||||
{
|
||||
var radio = buffer.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
|
||||
//Debug.Print("fix:" + radio.fixedp + " noise:" + radio.noise + " remnoise:" + radio.remnoise + " remrssi:" + radio.remrssi + " rssi:" + radio.rssi + " rxerr:" + radio.rxerrors + " txbuf:" + radio.txbuf);
|
||||
remrssi = radio.remrssi;
|
||||
rssi = radio.rssi;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeRawImuPacket(byte[] buffer)
|
||||
{
|
||||
var imu = buffer.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
|
||||
|
||||
gx = imu.xgyro;
|
||||
gy = imu.ygyro;
|
||||
gz = imu.zgyro;
|
||||
|
||||
ax = imu.xacc;
|
||||
ay = imu.yacc;
|
||||
az = imu.zacc;
|
||||
|
||||
mx = imu.xmag;
|
||||
my = imu.ymag;
|
||||
mz = imu.zmag;
|
||||
|
||||
var handler = ReceiveSensorEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() =>
|
||||
{
|
||||
handler(this);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeRCChannelsRawPacket(byte[] buffer)
|
||||
{
|
||||
var rcin = buffer.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
|
||||
|
||||
ch1in = rcin.chan1_raw;
|
||||
ch2in = rcin.chan2_raw;
|
||||
ch3in = rcin.chan3_raw;
|
||||
ch4in = rcin.chan4_raw;
|
||||
ch5in = rcin.chan5_raw;
|
||||
ch6in = rcin.chan6_raw;
|
||||
ch7in = rcin.chan7_raw;
|
||||
ch8in = rcin.chan8_raw;
|
||||
if (bInitChannel == false)
|
||||
{
|
||||
bInitChannel = true;
|
||||
}
|
||||
|
||||
//percent
|
||||
rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
|
||||
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
private void AnalyzeScaledImu2Packet(byte[] buffer)
|
||||
{
|
||||
var imu2 = buffer.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
|
||||
gx2 = imu2.xgyro;
|
||||
gy2 = imu2.ygyro;
|
||||
gz2 = imu2.zgyro;
|
||||
|
||||
ax2 = imu2.xacc;
|
||||
ay2 = imu2.yacc;
|
||||
az2 = imu2.zacc;
|
||||
|
||||
mx2 = imu2.xmag;
|
||||
my2 = imu2.ymag;
|
||||
mz2 = imu2.zmag;
|
||||
|
||||
var handler = ReceiveScaleImu2Event;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() => handler(this));
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeSensorOffsetsPacket(byte[] buffer)
|
||||
{
|
||||
var sensofs = buffer.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
|
||||
|
||||
mag_ofs_x = sensofs.mag_ofs_x;
|
||||
mag_ofs_y = sensofs.mag_ofs_y;
|
||||
mag_ofs_z = sensofs.mag_ofs_z;
|
||||
mag_declination = sensofs.mag_declination;
|
||||
|
||||
raw_press = sensofs.raw_press;
|
||||
raw_temp = sensofs.raw_temp;
|
||||
|
||||
gyro_cal_x = sensofs.gyro_cal_x;
|
||||
gyro_cal_y = sensofs.gyro_cal_y;
|
||||
gyro_cal_z = sensofs.gyro_cal_z;
|
||||
|
||||
accel_cal_x = sensofs.accel_cal_x;
|
||||
accel_cal_y = sensofs.accel_cal_y;
|
||||
accel_cal_z = sensofs.accel_cal_z;
|
||||
|
||||
var handler = ReceiveSensorEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() =>
|
||||
{
|
||||
handler(this);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeStatusTextPacket(byte[] buffer)
|
||||
{
|
||||
var stext = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
|
||||
var length = Array.IndexOf<byte>(stext.text, 0);
|
||||
var log = Encoding.ASCII.GetString(stext.text, 0, length);
|
||||
var handler = GetLogDataEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() => handler(log));
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeSysStatusPacket(byte[] buffer)
|
||||
{
|
||||
var sysstatus = buffer.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
|
||||
battery_voltage = sysstatus.voltage_battery;
|
||||
battery_remaining = sysstatus.battery_remaining;
|
||||
current_battery = sysstatus.current_battery / 100.0f;
|
||||
|
||||
var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
|
||||
var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
|
||||
|
||||
if (sensors_health.gps != sensors_enabled.gps)
|
||||
{
|
||||
bGPSBadHealth = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
bGPSBadHealth = false;
|
||||
}
|
||||
if (sensors_health.gyro != sensors_enabled.gyro)
|
||||
{
|
||||
bGyroBadHealth = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
bGyroBadHealth = false;
|
||||
}
|
||||
if (sensors_health.accelerometer != sensors_enabled.accelerometer)
|
||||
{
|
||||
bAccel = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
bAccel = false;
|
||||
}
|
||||
if (sensors_health.compass != sensors_enabled.compass)
|
||||
{
|
||||
bCompass = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
bCompass = false;
|
||||
}
|
||||
if (sensors_health.barometer != sensors_enabled.barometer)
|
||||
{
|
||||
bBarometer = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
bBarometer = false;
|
||||
}
|
||||
FlightDistance2D = sysstatus.errors_count2;
|
||||
var handler = ReceiveSysStatusEvent;
|
||||
if (handler != null)
|
||||
{
|
||||
RunOnUIThread(() =>
|
||||
{
|
||||
handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
private void AnalyzeVfrHudPacket(byte[] buffer)
|
||||
{
|
||||
var vfr = buffer.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
|
||||
groundspeed = vfr.groundspeed;
|
||||
airspeed = vfr.airspeed;
|
||||
altorigin = vfr.alt; // this might include baro
|
||||
ch3percent = vfr.throttle;
|
||||
heading = vfr.heading;
|
||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 飞控返回数据 处理通信模块发过来的28个字节数据
|
||||
/// </summary>
|
||||
/// <param name="buffer"></param>
|
||||
public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version)
|
||||
{
|
||||
var info = buffer.ByteArrayToStructure<MavComm.comm_copter_info>(0);
|
||||
lat = info.lat * 1.0e-7;
|
||||
lng = info.lng * 1.0e-7;
|
||||
commModuleMode = (uint)info.control_mode;
|
||||
gpsstatus = (byte)info.gps_status;
|
||||
errorcode= (byte)info.error_code;
|
||||
precheckok = info.rpecheck_fault == 0;
|
||||
gpsalt = ((float)info.alt) / 1000;
|
||||
satcount = info.gps_num_sats;
|
||||
isUnlocked = info.lock_status == 1;
|
||||
|
||||
battery_voltage = ((float)info.battery_voltage) / 1000;
|
||||
|
||||
heading = (short)((info.heading / 100) % 360);
|
||||
|
||||
commModuleVersion = version;
|
||||
|
||||
retain = info.retain;
|
||||
RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -34,6 +34,7 @@
|
||||
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\CopterControllerBase.cs" />
|
||||
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\CopterControllerManager.cs" />
|
||||
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\FlyToController.cs" />
|
||||
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\GuidController.cs" />
|
||||
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\KeyboardController.cs" />
|
||||
<Compile Include="$(MSBuildThisFileDirectory)CopterManagement\CopterFactory.cs" />
|
||||
<Compile Include="$(MSBuildThisFileDirectory)CopterManagement\SingleCopterManager.cs" />
|
||||
|
||||
@ -17,7 +17,7 @@ namespace Plane.Protocols
|
||||
public const int MAVLINK_LITTLE_ENDIAN = 1;
|
||||
public const int MAVLINK_BIG_ENDIAN = 0;
|
||||
|
||||
//飞控4.0以后 254 - 240
|
||||
//协议头 飞控4.0以后 254 - 240 Mavlink头 FE 改为 F0
|
||||
//public const byte MAVLINK_STX = 254;
|
||||
public const byte MAVLINK_STX = 240;
|
||||
|
||||
@ -29,7 +29,7 @@ namespace Plane.Protocols
|
||||
|
||||
public const bool MAVLINK_NEED_BYTE_SWAP = (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN);
|
||||
|
||||
// 林俊清, 20150721: 消息长度表未被使用，而且没更新，删除。
|
||||
// 王海, 20150721: 消息长度表未被使用，而且没更新，删除。
|
||||
//public static readonly byte[] MAVLINK_MESSAGE_LENGTHS = new byte[] { 9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0 };
|
||||
|
||||
//public static readonly byte[] MAVLINK_MESSAGE_CRCS = new byte[] {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 59, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0 };
|
||||
@ -79,15 +79,15 @@ namespace Plane.Protocols
|
||||
///<summary> Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| </summary>
|
||||
TAKEOFF = 22,
|
||||
/// <summary>
|
||||
/// // 林俊清, 20160331, 来自郭老师，据说是传文件用的。
|
||||
/// // 王海, 20160331, 来自王海1，据说是传文件用的。
|
||||
/// </summary>
|
||||
TELL_COPTER = 23,
|
||||
/// <summary>
|
||||
/// // 林俊清, 20160331, 来自郭老师的邮件，作用未知。
|
||||
/// // 王海, 20160331, 来自王海1的邮件，作用未知。
|
||||
/// </summary>
|
||||
FILE_TRANS_MODE = 24,
|
||||
/// <summary>
|
||||
/// // 林俊清, 20160331, 来自郭老师的邮件，用于 RF 测试。
|
||||
/// // 王海, 20160331, 来自王海1的邮件，用于 RF 测试。
|
||||
/// </summary>
|
||||
RF_TEST = 25,
|
||||
///<summary> Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| </summary>
|
||||
@ -1366,7 +1366,7 @@ namespace Plane.Protocols
|
||||
/// <summary> Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) </summary>
|
||||
public byte type;
|
||||
/// <summary> Autopilot type / class. defined in MAV_AUTOPILOT ENUM
|
||||
/// // 林俊清, 20160311, 亿航用作切过 GPS 模式的标志。</summary>
|
||||
/// // 王海, 20160311, 亿航用作切过 GPS 模式的标志。</summary>
|
||||
public byte autopilot;
|
||||
/// <summary> System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h </summary>
|
||||
public byte base_mode;
|
||||
|
||||
@ -1,160 +1,187 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text;
|
||||
|
||||
namespace Plane.Protocols
|
||||
{
|
||||
/*
|
||||
*
|
||||
* 通信模块协议
|
||||
*
|
||||
*/
|
||||
public class MavComm
|
||||
{
|
||||
/// <summary>
|
||||
/// 发送数据时的数据包头
|
||||
/// </summary>
|
||||
public const ushort COMM_SEND_PACKET_HEADER = 0x9551;
|
||||
/// <summary>
|
||||
/// 接受数据时的数据包头
|
||||
/// </summary>
|
||||
public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713;
|
||||
|
||||
|
||||
#region 命令类型 2字节
|
||||
//-----------------命令类型-----------------
|
||||
/// <summary>
|
||||
/// 查询状态
|
||||
/// </summary>
|
||||
public const short COMM_QUERY_STATUS = 0x00;
|
||||
|
||||
/// <summary>
|
||||
/// 变更飞机总数及参与者
|
||||
/// </summary>
|
||||
public const short COMM_SET_MAV_COUNT = 0x10;
|
||||
|
||||
/// <summary>
|
||||
/// 获取飞机详细信息
|
||||
/// </summary>
|
||||
public const short COMM_GET_COPTERS_COUNT = 0x20;
|
||||
|
||||
/// <summary>
|
||||
/// 时间同步，不改变当前模式
|
||||
/// </summary>
|
||||
public const short COMM_ASYN_TIME = 0x30;
|
||||
|
||||
/// <summary>
|
||||
/// 进入空闲模式
|
||||
/// </summary>
|
||||
public const short COMM_IDLE_MODE = 0x50;
|
||||
|
||||
/// <summary>
|
||||
/// 进入搜索模式，命名编号targetID memcpy(pdata,input,2);
|
||||
/// </summary>
|
||||
public const short COMM_SEARCH_MODE = 0x51;
|
||||
|
||||
/// <summary>
|
||||
/// 进入航点下载模式 (写航点)
|
||||
/// </summary>
|
||||
public const short COMM_DOWNLOAD_MODE = 0x52;
|
||||
|
||||
/// <summary>
|
||||
/// 下载模式通信
|
||||
/// </summary>
|
||||
public const short COMM_DOWNLOAD_COMM = 0x53;
|
||||
|
||||
/// <summary>
|
||||
/// 进入飞行模式（无负载）
|
||||
/// </summary>
|
||||
public const short COMM_FLIGHT_MODE = 0x54;
|
||||
|
||||
/// <summary>
|
||||
/// 进入飞行模式（有负载数据）
|
||||
/// </summary>
|
||||
public const short COMM_FLIGHT_LOAD_MODE = 0x55;
|
||||
|
||||
/// <summary>
|
||||
/// 通信模块
|
||||
/// </summary>
|
||||
public const short COMM_NOTIFICATION = 0x1234;
|
||||
|
||||
/// <summary>
|
||||
/// 空中升级(更新通信模块飞机端)
|
||||
/// </summary>
|
||||
public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
|
||||
|
||||
#endregion
|
||||
|
||||
public enum CommMode
|
||||
{
|
||||
IDLE = 0,
|
||||
SEARCH = 1,
|
||||
DOWNLOAD = 3,
|
||||
FLIGHT = 4
|
||||
}
|
||||
|
||||
public enum MessageType
|
||||
{
|
||||
STR = 0,
|
||||
SCAN2 = 2,
|
||||
SCAN3 = 3
|
||||
}
|
||||
|
||||
//search 搜索模式相关
|
||||
public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功
|
||||
public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功
|
||||
public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标
|
||||
public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误
|
||||
public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S
|
||||
|
||||
|
||||
public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息,
|
||||
//需要再后续等待一个成功消息，才算完成
|
||||
|
||||
public const ushort P2P_SEND_FAILED = 0x0600; //点对点通信发送失败
|
||||
|
||||
public const ushort ERROR_CODE_START = 0x0100;
|
||||
public const ushort ERROR_CODE_END = 0x03FF;
|
||||
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
|
||||
public struct comm_set_mav_count
|
||||
{
|
||||
public Int16 mav_count; //飞机总数
|
||||
};
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
|
||||
public struct comm_update_copter_module
|
||||
{
|
||||
public Int16 mav_count; //飞机总数
|
||||
public Int16 update_code;
|
||||
};
|
||||
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
|
||||
public struct comm_asyn_time
|
||||
{
|
||||
public Int64 time_stamp;
|
||||
};
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
|
||||
public struct comm_copter_info
|
||||
{
|
||||
public Int32 control_mode;
|
||||
public UInt32 lat;
|
||||
public UInt32 lng;
|
||||
public float retain;
|
||||
public Int32 alt;
|
||||
|
||||
public Int16 gps_status;
|
||||
|
||||
public byte lock_status;
|
||||
public byte gps_num_sats;
|
||||
public Int16 battery_voltage;
|
||||
public Int16 heading;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text;
|
||||
|
||||
namespace Plane.Protocols
|
||||
{
|
||||
/*
|
||||
*
|
||||
* 通信模块协议
|
||||
*
|
||||
*/
|
||||
public class MavComm
|
||||
{
|
||||
/// <summary>
|
||||
/// 发送数据时的数据包头
|
||||
/// </summary>
|
||||
public const ushort COMM_SEND_PACKET_HEADER = 0x9551;
|
||||
/// <summary>
|
||||
/// 接受数据时的数据包头
|
||||
/// </summary>
|
||||
public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713;
|
||||
|
||||
|
||||
#region 命令类型 2字节
|
||||
//-----------------命令类型-----------------
|
||||
/// <summary>
|
||||
/// 查询状态
|
||||
/// </summary>
|
||||
public const short COMM_QUERY_STATUS = 0x00;
|
||||
|
||||
/// <summary>
|
||||
/// 变更飞机总数及参与者
|
||||
/// </summary>
|
||||
public const short COMM_SET_MAV_COUNT = 0x10;
|
||||
|
||||
/// <summary>
|
||||
/// 获取飞机详细信息
|
||||
/// </summary>
|
||||
public const short COMM_GET_COPTERS_COUNT = 0x20;
|
||||
|
||||
/// <summary>
|
||||
/// 时间同步，不改变当前模式
|
||||
/// </summary>
|
||||
public const short COMM_ASYN_TIME = 0x30;
|
||||
|
||||
/// <summary>
|
||||
/// 进入空闲模式
|
||||
/// </summary>
|
||||
public const short COMM_IDLE_MODE = 0x50;
|
||||
|
||||
/// <summary>
|
||||
/// 进入搜索模式，命名编号targetID memcpy(pdata,input,2);
|
||||
/// </summary>
|
||||
public const short COMM_SEARCH_MODE = 0x51;
|
||||
|
||||
/// <summary>
|
||||
/// 进入航点下载模式 (写航点)
|
||||
/// </summary>
|
||||
public const short COMM_DOWNLOAD_MODE = 0x52;
|
||||
|
||||
/// <summary>
|
||||
/// 下载模式通信
|
||||
/// </summary>
|
||||
public const short COMM_DOWNLOAD_COMM = 0x53;
|
||||
|
||||
/// <summary>
|
||||
/// 进入飞行模式（无负载）
|
||||
/// </summary>
|
||||
public const short COMM_FLIGHT_MODE = 0x54;
|
||||
|
||||
/// <summary>
|
||||
/// 进入飞行模式（有负载数据）
|
||||
/// </summary>
|
||||
public const short COMM_FLIGHT_LOAD_MODE = 0x55;
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 发送非针对飞控的内部控制指令
|
||||
/// </summary>
|
||||
public const short COMM_TO_MODULE = 0x5F;
|
||||
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 通信模块
|
||||
/// </summary>
|
||||
public const short COMM_NOTIFICATION = 0x1234;
|
||||
|
||||
/// <summary>
|
||||
/// 空中升级(更新通信模块飞机端)
|
||||
/// </summary>
|
||||
public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
|
||||
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 测试模块
|
||||
/// </summary>
|
||||
public const short COMM_TEST_MODULE = 0x3C;
|
||||
|
||||
|
||||
|
||||
#endregion
|
||||
|
||||
public enum CommMode
|
||||
{
|
||||
IDLE = 0,
|
||||
SEARCH = 1,
|
||||
DOWNLOAD = 3,
|
||||
FLIGHT = 4
|
||||
}
|
||||
|
||||
public enum MessageType
|
||||
{
|
||||
STR = 0,
|
||||
SCAN2 = 2,
|
||||
SCAN3 = 3
|
||||
}
|
||||
|
||||
//search 搜索模式相关
|
||||
public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功
|
||||
public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功
|
||||
public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标
|
||||
public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误
|
||||
public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S
|
||||
|
||||
|
||||
public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息,
|
||||
//需要再后续等待一个成功消息，才算完成
|
||||
|
||||
public const ushort P2P_SEND_FAILED = 0x0600; //点对点通信发送失败
|
||||
|
||||
public const ushort ERROR_CODE_START = 0x0100;
|
||||
public const ushort ERROR_CODE_END = 0x03FF;
|
||||
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
|
||||
public struct comm_set_mav_count
|
||||
{
|
||||
public Int16 mav_count; //飞机总数
|
||||
};
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
|
||||
public struct comm_update_copter_module
|
||||
{
|
||||
public Int16 mav_count; //飞机总数
|
||||
public Int16 update_code;
|
||||
};
|
||||
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
|
||||
public struct comm_asyn_time
|
||||
{
|
||||
public Int64 time_stamp;
|
||||
};
|
||||
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
|
||||
public struct comm_copter_info
|
||||
{
|
||||
// public Int32 control_mode; //飞行模式(自动，悬停，定高，返航)
|
||||
public byte control_mode; //飞行模式(自动，悬停，定高，返航)
|
||||
public byte rpecheck_fault; //是否有 预解锁错误
|
||||
public byte reserveddata1; //保留
|
||||
public byte reserveddata2; //保留
|
||||
|
||||
|
||||
public UInt32 lat; // 经度 in 1E7 degrees
|
||||
public UInt32 lng; // 维度 in 1E7 degrees
|
||||
public float retain; // 写参数序列号,返回版本号等不特定返回数据
|
||||
public Int32 alt; // millimeters above home
|
||||
|
||||
|
||||
public byte gps_status; //锁定类型 (无锁定,3D锁定，RTK浮点，RTK固定)
|
||||
public byte error_code; //错误号，0表示无错误 --放到低位为了和之前兼容
|
||||
|
||||
|
||||
|
||||
public byte lock_status; //以及是否解锁
|
||||
public byte gps_num_sats; // 卫星数量
|
||||
public Int16 battery_voltage; // 电池电压mV
|
||||
public UInt16 heading; //方向角度
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -44,7 +44,7 @@ namespace Plane.Protocols
|
||||
Marshal.Copy(bytearray, startoffset, i, len);
|
||||
packet = Marshal.PtrToStructure<TMavlinkPacket>(i);
|
||||
}
|
||||
// 林俊清, 20151026, 改为不吞异常了。
|
||||
// 王海, 20151026, 改为不吞异常了。
|
||||
//catch
|
||||
//{
|
||||
// //log.Error("ByteArrayToStructure FAIL", ex);
|
||||
|
||||
@ -43,7 +43,7 @@ namespace Plane.Protocols
|
||||
Marshal.Copy(bytearray, startoffset, i, len);
|
||||
packet = MarshalEx.PtrToStructure<TMavlinkPacket>(i);
|
||||
}
|
||||
// 林俊清, 20151026, 改为不吞异常了。
|
||||
// 王海, 20151026, 改为不吞异常了。
|
||||
//catch
|
||||
//{
|
||||
// //log.Error("ByteArrayToStructure FAIL", ex);
|
||||
|
||||
@ -39,13 +39,13 @@ namespace TaskTools.HIL
|
||||
self.z);
|
||||
}
|
||||
|
||||
// 林俊清，20150702：删除 MissionPlanner.Utilities.dll。
|
||||
// 王海，20150702：删除 MissionPlanner.Utilities.dll。
|
||||
//public static implicit operator Vector3(PointLatLngAlt a)
|
||||
//{
|
||||
// return new Vector3(a.Lat,a.Lng,a.Alt);
|
||||
//}
|
||||
|
||||
// 林俊清，20150702：删除 GMap.NET.Core.dll（仅此处使用其 PointLatLng）。
|
||||
// 王海，20150702：删除 GMap.NET.Core.dll（仅此处使用其 PointLatLng）。
|
||||
|
||||
//public static implicit operator Vector3(PointLatLng a)
|
||||
//{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user