Compare commits
1 Commits
Dev
...
兼容4.1.2以前的
| Author | SHA1 | Date | |
|---|---|---|---|
| f74435bf78 |
@ -11,7 +11,7 @@ namespace Plane.Copters
|
|||||||
Task CircleAsync();
|
Task CircleAsync();
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
/// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="bootloaderMode"></param>
|
/// <param name="bootloaderMode"></param>
|
||||||
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
||||||
|
|||||||
@ -34,17 +34,15 @@
|
|||||||
<WarningLevel>4</WarningLevel>
|
<WarningLevel>4</WarningLevel>
|
||||||
<DocumentationFile>bin\Release\PlaneGcsSdk.Contract_Private.xml</DocumentationFile>
|
<DocumentationFile>bin\Release\PlaneGcsSdk.Contract_Private.xml</DocumentationFile>
|
||||||
</PropertyGroup>
|
</PropertyGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<!-- A reference to the entire .NET Framework is automatically included -->
|
||||||
|
</ItemGroup>
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<Compile Include="Copters\DataStreamReceivedEventArgs.cs" />
|
<Compile Include="Copters\DataStreamReceivedEventArgs.cs" />
|
||||||
<Compile Include="Copters\DataStreamType.cs" />
|
<Compile Include="Copters\DataStreamType.cs" />
|
||||||
<Compile Include="Copters\ICopterActions.cs" />
|
<Compile Include="Copters\ICopterActions.cs" />
|
||||||
<Compile Include="Properties\AssemblyInfo.cs" />
|
<Compile Include="Properties\AssemblyInfo.cs" />
|
||||||
</ItemGroup>
|
</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="..\PlaneGcsSdk.Contract_Shared\PlaneGcsSdk.Contract_Shared.projitems" Label="Shared" />
|
||||||
<Import Project="$(MSBuildExtensionsPath32)\Microsoft\Portable\$(TargetFrameworkVersion)\Microsoft.Portable.CSharp.targets" />
|
<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.
|
<!-- To modify your build process, add your task inside one of the targets below and uncomment it.
|
||||||
|
|||||||
@ -23,10 +23,5 @@
|
|||||||
/// 最高速度，单位为 m/s。
|
/// 最高速度，单位为 m/s。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const float MAX_VEL = 5f;
|
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,9 +31,7 @@ namespace Plane.Copters
|
|||||||
/// <param name="lng">经度。</param>
|
/// <param name="lng">经度。</param>
|
||||||
/// <param name="alt">相对于解锁点的高度。</param>
|
/// <param name="alt">相对于解锁点的高度。</param>
|
||||||
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
||||||
/// flytime =飞行时间 秒
|
Task FlyToAsync(double lat, double lng, float alt);
|
||||||
/// flytype 0;匀速 1;加减速 2;单加速 3;单减速
|
|
||||||
Task FlyToAsync(double lat, double lng, float alt,float flytime=0,int flytype = 0);
|
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 切换到 <see cref="CopterState.HoverMode"/> 并悬停。此操作需要使用 GPS 定位，卫星数不足导致定位不准时非常危险。
|
/// 切换到 <see cref="CopterState.HoverMode"/> 并悬停。此操作需要使用 GPS 定位，卫星数不足导致定位不准时非常危险。
|
||||||
|
|||||||
@ -14,13 +14,5 @@ namespace Plane.Copters
|
|||||||
/// </summary>
|
/// </summary>
|
||||||
float GroundAlt { get; set; }
|
float GroundAlt { get; set; }
|
||||||
float RetainInt{ get; }
|
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,6 +1,5 @@
|
|||||||
using Plane.Geography;
|
using Plane.Geography;
|
||||||
using System;
|
using System;
|
||||||
using System.Drawing;
|
|
||||||
|
|
||||||
namespace Plane.Copters
|
namespace Plane.Copters
|
||||||
{
|
{
|
||||||
@ -188,19 +187,5 @@ namespace Plane.Copters
|
|||||||
/// LED颜色
|
/// LED颜色
|
||||||
/// </summary>
|
/// </summary>
|
||||||
string LEDColor { get; set; }
|
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? latitude = null, //23.14973333,
|
||||||
double? longitude = null, //113.40974166,
|
double? longitude = null, //113.40974166,
|
||||||
float? altitude = null, //0,
|
float? altitude = null, //0,
|
||||||
string name = null, //"王海的飞行器",
|
string name = null, //"林俊清的飞行器",
|
||||||
byte? batteryPer = null, //10,
|
byte? batteryPer = null, //10,
|
||||||
short? heading = null, //33,
|
short? heading = null, //33,
|
||||||
bool? isConnected = null, //true,
|
bool? isConnected = null, //true,
|
||||||
|
|||||||
@ -55,45 +55,6 @@ namespace Plane.Geography
|
|||||||
{
|
{
|
||||||
return CalcDistance(l1.Latitude, l1.Longitude, l1.Altitude, l2.Latitude, l2.Longitude, l2.Altitude);
|
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>
|
/// <summary>
|
||||||
/// 计算空间中两点之间的距离，单位为米。
|
/// 计算空间中两点之间的距离，单位为米。
|
||||||
|
|||||||
@ -6,7 +6,7 @@ namespace Plane.Copters
|
|||||||
public partial class FakeCopter
|
public partial class FakeCopter
|
||||||
{
|
{
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
/// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="bootloaderMode"></param>
|
/// <param name="bootloaderMode"></param>
|
||||||
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
||||||
|
|||||||
@ -10,7 +10,7 @@ namespace Plane.Copters
|
|||||||
public PlaneCopter InternalCopter { get { return _internalCopter; } }
|
public PlaneCopter InternalCopter { get { return _internalCopter; } }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
/// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="bootloaderMode"></param>
|
/// <param name="bootloaderMode"></param>
|
||||||
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
||||||
|
|||||||
@ -6,7 +6,7 @@ namespace Plane.Copters
|
|||||||
partial class PlaneCopter
|
partial class PlaneCopter
|
||||||
{
|
{
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// // 王海, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
/// // 林俊清, 20160122, 不明确。目测是发送重启命令，在刷固件之前用到。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="bootloaderMode"></param>
|
/// <param name="bootloaderMode"></param>
|
||||||
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
||||||
|
|||||||
@ -1,81 +1,79 @@
|
|||||||
<?xml version="1.0" encoding="utf-8"?>
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
<Project ToolsVersion="14.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
<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')" />
|
<Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
|
||||||
<PropertyGroup>
|
<PropertyGroup>
|
||||||
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
|
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
|
||||||
<Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
|
<Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
|
||||||
<ProjectGuid>{0111EB6E-72E3-499C-A3BA-022F5BBC4CAF}</ProjectGuid>
|
<ProjectGuid>{0111EB6E-72E3-499C-A3BA-022F5BBC4CAF}</ProjectGuid>
|
||||||
<OutputType>Library</OutputType>
|
<OutputType>Library</OutputType>
|
||||||
<AppDesignerFolder>Properties</AppDesignerFolder>
|
<AppDesignerFolder>Properties</AppDesignerFolder>
|
||||||
<RootNamespace>Plane</RootNamespace>
|
<RootNamespace>Plane</RootNamespace>
|
||||||
<AssemblyName>PlaneGcsSdk_Private_NET46</AssemblyName>
|
<AssemblyName>PlaneGcsSdk_Private_NET46</AssemblyName>
|
||||||
<TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
|
<TargetFrameworkVersion>v4.6</TargetFrameworkVersion>
|
||||||
<FileAlignment>512</FileAlignment>
|
<FileAlignment>512</FileAlignment>
|
||||||
<TargetFrameworkProfile />
|
</PropertyGroup>
|
||||||
</PropertyGroup>
|
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
|
||||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
|
<DebugSymbols>true</DebugSymbols>
|
||||||
<DebugSymbols>true</DebugSymbols>
|
<DebugType>full</DebugType>
|
||||||
<DebugType>full</DebugType>
|
<Optimize>false</Optimize>
|
||||||
<Optimize>false</Optimize>
|
<OutputPath>bin\Debug\</OutputPath>
|
||||||
<OutputPath>bin\Debug\</OutputPath>
|
<DefineConstants>TRACE;DEBUG;PRIVATE</DefineConstants>
|
||||||
<DefineConstants>TRACE;DEBUG;PRIVATE</DefineConstants>
|
<ErrorReport>prompt</ErrorReport>
|
||||||
<ErrorReport>prompt</ErrorReport>
|
<WarningLevel>4</WarningLevel>
|
||||||
<WarningLevel>4</WarningLevel>
|
</PropertyGroup>
|
||||||
</PropertyGroup>
|
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
|
||||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
|
<DebugType>pdbonly</DebugType>
|
||||||
<DebugType>pdbonly</DebugType>
|
<Optimize>true</Optimize>
|
||||||
<Optimize>true</Optimize>
|
<OutputPath>bin\Release\</OutputPath>
|
||||||
<OutputPath>bin\Release\</OutputPath>
|
<DefineConstants>TRACE;PRIVATE</DefineConstants>
|
||||||
<DefineConstants>TRACE;PRIVATE</DefineConstants>
|
<ErrorReport>prompt</ErrorReport>
|
||||||
<ErrorReport>prompt</ErrorReport>
|
<WarningLevel>4</WarningLevel>
|
||||||
<WarningLevel>4</WarningLevel>
|
<DocumentationFile>bin\Release\PlaneGcsSdk_Private_NET46.xml</DocumentationFile>
|
||||||
<DocumentationFile>bin\Release\PlaneGcsSdk_Private_NET46.xml</DocumentationFile>
|
</PropertyGroup>
|
||||||
</PropertyGroup>
|
<ItemGroup>
|
||||||
<ItemGroup>
|
<Reference Include="PresentationCore" />
|
||||||
<Reference Include="PresentationCore" />
|
<Reference Include="System" />
|
||||||
<Reference Include="System" />
|
<Reference Include="System.Core" />
|
||||||
<Reference Include="System.Core" />
|
<Reference Include="System.Xml.Linq" />
|
||||||
<Reference Include="System.Drawing" />
|
<Reference Include="System.Data.DataSetExtensions" />
|
||||||
<Reference Include="System.Xml.Linq" />
|
<Reference Include="Microsoft.CSharp" />
|
||||||
<Reference Include="System.Data.DataSetExtensions" />
|
<Reference Include="System.Data" />
|
||||||
<Reference Include="Microsoft.CSharp" />
|
<Reference Include="System.Net.Http" />
|
||||||
<Reference Include="System.Data" />
|
<Reference Include="System.Xml" />
|
||||||
<Reference Include="System.Net.Http" />
|
<Reference Include="WindowsBase" />
|
||||||
<Reference Include="System.Xml" />
|
</ItemGroup>
|
||||||
<Reference Include="WindowsBase" />
|
<ItemGroup>
|
||||||
</ItemGroup>
|
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\CopterImplSharedPart.Private.cs">
|
||||||
<ItemGroup>
|
<Link>Copters\CopterImplSharedPart.Private.cs</Link>
|
||||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\CopterImplSharedPart.Private.cs">
|
</Compile>
|
||||||
<Link>Copters\CopterImplSharedPart.Private.cs</Link>
|
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PlaneCopter.Private.cs">
|
||||||
</Compile>
|
<Link>Copters\PlaneCopter.Private.cs</Link>
|
||||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PlaneCopter.Private.cs">
|
</Compile>
|
||||||
<Link>Copters\PlaneCopter.Private.cs</Link>
|
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PLCopter.Private.cs">
|
||||||
</Compile>
|
<Link>Copters\EHCopter.Private.cs</Link>
|
||||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PLCopter.Private.cs">
|
</Compile>
|
||||||
<Link>Copters\EHCopter.Private.cs</Link>
|
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\FakeCopter.Private.cs">
|
||||||
</Compile>
|
<Link>Copters\FakeCopter.Private.cs</Link>
|
||||||
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\FakeCopter.Private.cs">
|
</Compile>
|
||||||
<Link>Copters\FakeCopter.Private.cs</Link>
|
<Compile Include="Properties\AssemblyInfo.cs" />
|
||||||
</Compile>
|
</ItemGroup>
|
||||||
<Compile Include="Properties\AssemblyInfo.cs" />
|
<ItemGroup>
|
||||||
</ItemGroup>
|
<ProjectReference Include="..\..\Plane.Libraries\Plane.Windows.Messages\Plane.Windows.Messages.csproj">
|
||||||
<ItemGroup>
|
<Project>{413C18E2-235F-4E15-B5C1-633657DE5D7A}</Project>
|
||||||
<ProjectReference Include="..\..\Plane.Libraries\Plane.Windows.Messages\Plane.Windows.Messages.csproj">
|
<Name>Plane.Windows.Messages</Name>
|
||||||
<Project>{413C18E2-235F-4E15-B5C1-633657DE5D7A}</Project>
|
</ProjectReference>
|
||||||
<Name>Plane.Windows.Messages</Name>
|
<ProjectReference Include="..\PlaneGcsSdk.Contract_Private\PlaneGcsSdk.Contract_Private.csproj">
|
||||||
</ProjectReference>
|
<Project>{47141894-ece3-48ca-8dcf-ca751bda231e}</Project>
|
||||||
<ProjectReference Include="..\PlaneGcsSdk.Contract_Private\PlaneGcsSdk.Contract_Private.csproj">
|
<Name>PlaneGcsSdk.Contract_Private</Name>
|
||||||
<Project>{47141894-ece3-48ca-8dcf-ca751bda231e}</Project>
|
</ProjectReference>
|
||||||
<Name>PlaneGcsSdk.Contract_Private</Name>
|
</ItemGroup>
|
||||||
</ProjectReference>
|
<Import Project="..\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.projitems" Label="Shared" />
|
||||||
</ItemGroup>
|
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
|
||||||
<Import Project="..\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.projitems" Label="Shared" />
|
<!-- To modify your build process, add your task inside one of the targets below and uncomment it.
|
||||||
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
|
Other similar extension points exist, see Microsoft.Common.targets.
|
||||||
<!-- To modify your build process, add your task inside one of the targets below and uncomment it.
|
<Target Name="BeforeBuild">
|
||||||
Other similar extension points exist, see Microsoft.Common.targets.
|
</Target>
|
||||||
<Target Name="BeforeBuild">
|
<Target Name="AfterBuild">
|
||||||
</Target>
|
</Target>
|
||||||
<Target Name="AfterBuild">
|
-->
|
||||||
</Target>
|
|
||||||
-->
|
|
||||||
</Project>
|
</Project>
|
||||||
@ -1,138 +1,116 @@
|
|||||||
#if !NETFX_CORE
|
#if !NETFX_CORE
|
||||||
|
|
||||||
using System;
|
using System;
|
||||||
using System.Net;
|
using System.Net;
|
||||||
using System.Net.Sockets;
|
using System.Net.Sockets;
|
||||||
using System.Threading.Tasks;
|
using System.Threading.Tasks;
|
||||||
|
|
||||||
namespace Plane.Communication
|
namespace Plane.Communication
|
||||||
{
|
{
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 提供作为 TCP 客户端通信的能力。
|
/// 提供作为 TCP 客户端通信的能力。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public class TcpConnection : TcpConnectionBase
|
public class TcpConnection : TcpConnectionBase
|
||||||
{
|
{
|
||||||
private string _remoteHostname;
|
private string _remoteHostname;
|
||||||
const int tcpReceiveBufferSize = 20 * 1024; //40 * 1024,
|
|
||||||
const int tcpReceiveTimeout = 1200;
|
private int _remotePort;
|
||||||
|
|
||||||
private int _remotePort;
|
public TcpConnection(string remoteHostname, int remotePort = 5250)
|
||||||
|
{
|
||||||
public TcpConnection(string remoteHostname, int remotePort = 5250)
|
_remoteHostname = remoteHostname;
|
||||||
{
|
_remotePort = remotePort;
|
||||||
_remoteHostname = remoteHostname;
|
_client = new TcpClient
|
||||||
_remotePort = remotePort;
|
{
|
||||||
_client = new TcpClient
|
ReceiveBufferSize = 40 * 1024,
|
||||||
{
|
ReceiveTimeout = 1200
|
||||||
ReceiveBufferSize = tcpReceiveBufferSize,
|
};
|
||||||
ReceiveTimeout = tcpReceiveTimeout
|
}
|
||||||
};
|
|
||||||
}
|
public void Open()
|
||||||
|
{
|
||||||
public void Open()
|
if (!IsOpen)
|
||||||
{
|
{
|
||||||
if (!IsOpen)
|
try
|
||||||
{
|
{
|
||||||
try
|
_client.Connect(_remoteHostname, _remotePort);
|
||||||
{
|
}
|
||||||
_client.Connect(_remoteHostname, _remotePort);
|
catch (SocketException)
|
||||||
}
|
{
|
||||||
catch (SocketException)
|
CreateClientAndConnect();
|
||||||
{
|
}
|
||||||
CreateClientAndConnect();
|
catch (ObjectDisposedException)
|
||||||
}
|
{
|
||||||
catch (ObjectDisposedException)
|
CreateClientAndConnect();
|
||||||
{
|
}
|
||||||
CreateClientAndConnect();
|
_isBroken = false;
|
||||||
}
|
}
|
||||||
_isBroken = false;
|
|
||||||
}
|
_stream = _client.GetStream();
|
||||||
|
}
|
||||||
_stream = _client.GetStream();
|
|
||||||
}
|
public async Task<bool> BindIP(string ip)
|
||||||
|
{
|
||||||
public async Task<bool> BindIP(string ip)
|
bool bind = false;
|
||||||
{
|
try
|
||||||
bool bind = false;
|
{
|
||||||
try
|
IPAddress IPLocal = IPAddress.Parse(ip);
|
||||||
{
|
_client.Client.Bind(new IPEndPoint(IPLocal, 0));
|
||||||
IPAddress IPLocal = IPAddress.Parse(ip);
|
bind = true;
|
||||||
_client.Client.Bind(new IPEndPoint(IPLocal, 0));
|
}
|
||||||
bind = true;
|
catch
|
||||||
}
|
{
|
||||||
catch
|
bind = false;
|
||||||
{
|
}
|
||||||
bind = false;
|
await Task.Delay(10).ConfigureAwait(false);
|
||||||
}
|
return bind;
|
||||||
await Task.Delay(10).ConfigureAwait(false);
|
}
|
||||||
return bind;
|
|
||||||
}
|
public override async Task OpenAsync()
|
||||||
|
{
|
||||||
public override async Task OpenAsync()
|
string logstr;
|
||||||
{
|
if (!IsOpen)
|
||||||
string logstr;
|
{
|
||||||
if (!IsOpen)
|
try
|
||||||
{
|
{
|
||||||
if (_client == null)
|
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
|
||||||
CreateClientAndConnect();
|
}
|
||||||
try
|
//屏蔽掉异常处理
|
||||||
{
|
//CreateClientAndConnectAsync会new一个TcpClient并且重新连接
|
||||||
var connectTask = _client.ConnectAsync(_remoteHostname, _remotePort);
|
//之前设置的TcpClient中Socket Bind会无效，在多网卡工作时会导致断线重连的时间特别长
|
||||||
if (await Task.WhenAny(connectTask, Task.Delay(5000)) == connectTask)
|
catch (SocketException e)
|
||||||
{
|
{
|
||||||
if (_client.Client != null) //需要测试
|
logstr= e.Message;
|
||||||
await connectTask.ConfigureAwait(false);
|
//await CreateClientAndConnectAsync();
|
||||||
}
|
}
|
||||||
else
|
catch (ObjectDisposedException)
|
||||||
{
|
{
|
||||||
// Connection timed out
|
//await CreateClientAndConnectAsync();
|
||||||
throw new TimeoutException("Connection timed out");
|
}
|
||||||
}
|
_isBroken = false;
|
||||||
}
|
}
|
||||||
catch (SocketException e)
|
_stream = _client.GetStream();
|
||||||
{
|
}
|
||||||
logstr = e.Message;
|
|
||||||
CloseClient(); // 关闭并清理客户端
|
private void CreateClientAndConnect()
|
||||||
}
|
{
|
||||||
catch (ObjectDisposedException)
|
_client = new TcpClient(_remoteHostname, _remotePort)
|
||||||
{
|
{
|
||||||
CloseClient(); // 关闭并清理客户端
|
ReceiveBufferSize = 40 * 1024,
|
||||||
}
|
ReceiveTimeout = 1200
|
||||||
catch (Exception)
|
};
|
||||||
{
|
}
|
||||||
CloseClient(); // 处理其他可能的异常
|
|
||||||
// throw; // 可选：重新抛出异常，以便调用者知道发生了错误
|
private async Task CreateClientAndConnectAsync()
|
||||||
}
|
{
|
||||||
_isBroken = false;
|
_client = new TcpClient
|
||||||
}
|
{
|
||||||
if (_client != null)
|
ReceiveBufferSize = 40 * 1024,
|
||||||
_stream = _client.GetStream();
|
ReceiveTimeout = 1200
|
||||||
}
|
};
|
||||||
private void CloseClient()
|
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
|
||||||
{
|
}
|
||||||
_client?.Close(); // 如果 _client 不为 null，则关闭连接
|
}
|
||||||
_client = null; // 将 _client 设置为 null，以便垃圾回收器可以回收它
|
}
|
||||||
}
|
|
||||||
|
#endif
|
||||||
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,10 +43,7 @@ namespace Plane.Communication
|
|||||||
{
|
{
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
// bool bret ;
|
return _client.Client != null && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
|
||||||
// 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)
|
catch (ObjectDisposedException)
|
||||||
{
|
{
|
||||||
|
|||||||
@ -21,7 +21,7 @@ namespace Plane.Communication
|
|||||||
|
|
||||||
private bool _isListening;
|
private bool _isListening;
|
||||||
|
|
||||||
// TODO: 王海, 20150909, 从配置文件中获取要监听的 IP 地址和端口号。
|
// TODO: 林俊清, 20150909, 从配置文件中获取要监听的 IP 地址和端口号。
|
||||||
private TcpListener _listener;
|
private TcpListener _listener;
|
||||||
|
|
||||||
private bool _shouldListen;
|
private bool _shouldListen;
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -1,213 +1,205 @@
|
|||||||
using Plane.Communication;
|
using Plane.Communication;
|
||||||
using Plane.Protocols;
|
using Plane.Protocols;
|
||||||
using Plane.Windows.Messages;
|
using Plane.Windows.Messages;
|
||||||
using System;
|
using System;
|
||||||
using System.Collections.Generic;
|
using System.Collections.Generic;
|
||||||
using System.Diagnostics;
|
using System.Diagnostics;
|
||||||
using System.Linq;
|
using System.Linq;
|
||||||
using System.Text;
|
using System.Text;
|
||||||
using System.Threading.Tasks;
|
using System.Threading.Tasks;
|
||||||
|
|
||||||
namespace Plane.CommunicationManagement
|
namespace Plane.CommunicationManagement
|
||||||
{
|
{
|
||||||
public partial class CommModuleManager
|
public partial class CommModuleManager
|
||||||
{
|
{
|
||||||
private int MissionStartCopterId = 0;
|
private int MissionStartCopterId = 0;
|
||||||
private int MissionSendCopterId = 0;
|
private int MissionSendCopterId = 0;
|
||||||
private int MissionErrorId = -1;
|
private int MissionErrorId = -1;
|
||||||
private int ErrorCode = 0;
|
private int ErrorCode = 0;
|
||||||
public event EventHandler<CommunicationReceiveCopterInfoEventArgs> CommunicationReceived;
|
public event EventHandler<CommunicationReceiveCopterInfoEventArgs> CommunicationReceived;
|
||||||
public event EventHandler<CommunicationCopterDisconnectEventArgs> CommunicationCopterDisconnect;
|
public event EventHandler<CommunicationCopterDisconnectEventArgs> CommunicationCopterDisconnect;
|
||||||
public event EventHandler<CommunicationConnectEventArgs> CommunicationConnected;
|
public event EventHandler<CommunicationConnectEventArgs> CommunicationConnected;
|
||||||
private Dictionary<int, CommWriteMissinState> missionWriteState = new Dictionary<int, CommWriteMissinState>();
|
private Dictionary<int, CommWriteMissinState> missionWriteState = new Dictionary<int, CommWriteMissinState>();
|
||||||
private static readonly object MissinLocker = new object();
|
private static readonly object MissinLocker = new object();
|
||||||
|
|
||||||
public Dictionary<int, CommWriteMissinState> MissionWriteState
|
public Dictionary<int, CommWriteMissinState> MissionWriteState
|
||||||
{
|
{
|
||||||
get {return missionWriteState; }
|
get {return missionWriteState; }
|
||||||
}
|
}
|
||||||
|
|
||||||
public void ClearMissionWriteState()
|
public void ClearMissionWriteState()
|
||||||
{
|
{
|
||||||
missionWriteState.Clear();
|
missionWriteState.Clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeMissionStartPacket(short copterId, byte[] buffer)
|
private void AnalyzeMissionStartPacket(short copterId, byte[] buffer)
|
||||||
{
|
{
|
||||||
short errorId = BitConverter.ToInt16(buffer, 0);
|
short errorId = BitConverter.ToInt16(buffer, 0);
|
||||||
MissionStartCopterId = copterId;
|
MissionStartCopterId = copterId;
|
||||||
MissionErrorId = errorId;
|
MissionErrorId = errorId;
|
||||||
if(errorId != 0)
|
if(errorId != 0)
|
||||||
Message.Show($"{copterId}:返回 = {errorId}");
|
Message.Show($"{copterId}:返回 = {errorId}");
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeStringPacket(short copterId, byte[] buffer)
|
private void AnalyzeStringPacket(short copterId, byte[] buffer)
|
||||||
{
|
{
|
||||||
int count = Array.IndexOf(buffer, (byte)0);
|
int count = Array.IndexOf(buffer, (byte)0);
|
||||||
string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube");
|
string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube");
|
||||||
Message.Show(msg);
|
Message.Show(msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer)
|
private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer)
|
||||||
{
|
{
|
||||||
ushort ret = BitConverter.ToUInt16(buffer, 0);
|
ushort ret = BitConverter.ToUInt16(buffer, 0);
|
||||||
switch (ret)
|
switch (ret)
|
||||||
{
|
{
|
||||||
case MavComm.SEARCH_FINDCOPTER:
|
case MavComm.SEARCH_FINDCOPTER:
|
||||||
Message.Show(copterId.ToString() + "---飞机开始相应");
|
Message.Show(copterId.ToString() + "---飞机开始相应");
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MavComm.SEARCH_COMPLETED:
|
case MavComm.SEARCH_COMPLETED:
|
||||||
Message.Show(copterId.ToString() + "---设置成功");
|
Message.Show(copterId.ToString() + "---设置成功");
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MavComm.SEARCH_OUTTIME:
|
case MavComm.SEARCH_OUTTIME:
|
||||||
Message.Show("超时无响应");
|
Message.Show("超时无响应");
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MavComm.MISSION_SEND_COMPLETED:
|
case MavComm.MISSION_SEND_COMPLETED:
|
||||||
MissionSendCopterId = copterId;
|
MissionSendCopterId = copterId;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MavComm.P2P_SEND_FAILED:
|
case MavComm.P2P_SEND_FAILED:
|
||||||
Message.Show("点对点通信发送失败");
|
Message.Show("点对点通信发送失败");
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END)
|
if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END)
|
||||||
{
|
{
|
||||||
ErrorCode = ret;
|
ErrorCode = ret;
|
||||||
Message.Show($"{copterId}--错误码：{ret}");
|
Message.Show($"{copterId}--错误码：{ret}");
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer)
|
private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer)
|
||||||
{
|
{
|
||||||
byte type = buffer[0];
|
byte type = buffer[0];
|
||||||
byte connectRet;
|
byte connectRet;
|
||||||
switch (type)
|
switch (type)
|
||||||
{
|
{
|
||||||
case 0x01:
|
case 0x01:
|
||||||
//connectRet = buffer[1];
|
//connectRet = buffer[1];
|
||||||
//if (connectRet == 0x0) //飞机连接上 不管飞控和通信模块的连接
|
//if (connectRet == 0x0) //飞机连接上 不管飞控和通信模块的连接
|
||||||
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
|
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
|
||||||
break;
|
break;
|
||||||
case 0x02:
|
case 0x02:
|
||||||
connectRet = buffer[1];
|
connectRet = buffer[1];
|
||||||
if (connectRet == 0x0) //飞机断开
|
if (connectRet == 0x0) //飞机断开
|
||||||
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 0x03:
|
case 0x03:
|
||||||
SaveMissionWriteStat(copterId, buffer[1]);
|
SaveMissionWriteStat(copterId, buffer[1]);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 0x04:
|
case 0x0e:
|
||||||
Message.Show($"飞机{copterId}:功率设置成功，新回传功率：{buffer[1]}");
|
if (copterId == 0)
|
||||||
break;
|
Message.Show("----------全部更新完成----------");
|
||||||
|
else
|
||||||
case 0x0e:
|
Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
|
||||||
if (copterId == 0)
|
break;
|
||||||
Message.Show("----------全部更新完成----------");
|
}
|
||||||
else
|
}
|
||||||
Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
|
|
||||||
break;
|
private void SaveMissionWriteStat(short copterId, byte wirteMissRet)
|
||||||
|
{
|
||||||
default:
|
Task.Run(async () =>
|
||||||
// Message.Show($"Comm3返回：{type}");
|
{
|
||||||
break;
|
lock (MissinLocker)
|
||||||
}
|
{
|
||||||
}
|
// var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]);
|
||||||
|
|
||||||
private void SaveMissionWriteStat(short copterId, byte wirteMissRet)
|
if (wirteMissRet == 0x20)
|
||||||
{
|
{
|
||||||
Task.Run(async () =>
|
if (missionWriteState.ContainsKey(copterId))
|
||||||
{
|
missionWriteState[copterId].WriteSucceed = true;
|
||||||
lock (MissinLocker)
|
Message.Show($"ID:{copterId}:航点写入成功");
|
||||||
{
|
}
|
||||||
// var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]);
|
if (wirteMissRet > 0x20 && wirteMissRet < 0x30)
|
||||||
|
{
|
||||||
if (wirteMissRet == 0x20)
|
if (missionWriteState.ContainsKey(copterId))
|
||||||
{
|
missionWriteState[copterId].WriteSucceed = false;
|
||||||
if (missionWriteState.ContainsKey(copterId))
|
Message.Show($"ID:{copterId}:航点写入失败");
|
||||||
missionWriteState[copterId].WriteSucceed = true;
|
}
|
||||||
Message.Show($"ID:{copterId}:航点写入成功");
|
}
|
||||||
}
|
await Task.Delay(10).ConfigureAwait(false);
|
||||||
if (wirteMissRet > 0x20 && wirteMissRet < 0x30)
|
}
|
||||||
{
|
);
|
||||||
if (missionWriteState.ContainsKey(copterId))
|
|
||||||
missionWriteState[copterId].WriteSucceed = false;
|
}
|
||||||
Message.Show($"ID:{copterId}:航点写入失败");
|
|
||||||
}
|
private void AnalyzeCopterInfosPacket(byte[] buffer)
|
||||||
}
|
{
|
||||||
await Task.Delay(10).ConfigureAwait(false);
|
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)
|
||||||
|
{
|
||||||
private void AnalyzeCopterInfosPacket(byte[] buffer)
|
return;
|
||||||
{
|
}
|
||||||
ushort beginNum = BitConverter.ToUInt16(buffer, 0);
|
|
||||||
ushort endNum = BitConverter.ToUInt16(buffer, 2);
|
int offset = 0;
|
||||||
int count = endNum - beginNum + 1;
|
for (int i = beginNum; i <= endNum; i++)
|
||||||
byte[] copter_packets = buffer.Skip(4).ToArray();
|
{
|
||||||
if (copter_packets.Length != count * 4)
|
byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray();
|
||||||
{
|
UInt16 state = BitConverter.ToUInt16(onePacket, 0);
|
||||||
return;
|
short copterId = (short)i;
|
||||||
}
|
switch (state >> 13)
|
||||||
|
{
|
||||||
int offset = 0;
|
//0B000
|
||||||
for (int i = beginNum; i <= endNum; i++)
|
case 0x0000 >> 13:
|
||||||
{
|
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
||||||
byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray();
|
break;
|
||||||
UInt16 state = BitConverter.ToUInt16(onePacket, 0);
|
//0B100
|
||||||
short copterId = (short)i;
|
case 0x8000 >> 13:
|
||||||
switch (state >> 13)
|
break;
|
||||||
{
|
//0B110
|
||||||
//0B000
|
case 0xC000 >> 13:
|
||||||
case 0x0000 >> 13:
|
//0B111
|
||||||
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
case 0xE000 >> 13:
|
||||||
break;
|
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
|
||||||
//0B100
|
break;
|
||||||
case 0x8000 >> 13:
|
}
|
||||||
break;
|
offset += 4;
|
||||||
//0B110
|
}
|
||||||
case 0xC000 >> 13:
|
}
|
||||||
//0B111
|
|
||||||
case 0xE000 >> 13:
|
private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer)
|
||||||
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
|
{
|
||||||
break;
|
byte[] packet = buffer.Take(28).ToArray();
|
||||||
}
|
byte[] state_packet = buffer.Skip(28).ToArray();
|
||||||
offset += 4;
|
UInt16 state = BitConverter.ToUInt16(state_packet,0);
|
||||||
}
|
byte version = state_packet[3];
|
||||||
}
|
switch (state >> 13)
|
||||||
|
{
|
||||||
private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer)
|
//0B000
|
||||||
{
|
case 0x0000 >> 13:
|
||||||
byte[] packet = buffer.Take(28).ToArray();
|
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
||||||
byte[] state_packet = buffer.Skip(28).ToArray();
|
break;
|
||||||
UInt16 state = BitConverter.ToUInt16(state_packet,0);
|
//0B100
|
||||||
byte version = state_packet[3];
|
case 0x8000 >> 13:
|
||||||
switch (state >> 13)
|
break;
|
||||||
{
|
//0B110
|
||||||
//0B000
|
case 0xC000 >> 13:
|
||||||
case 0x0000 >> 13:
|
//0B111
|
||||||
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
|
case 0xE000 >> 13:
|
||||||
break;
|
CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version));
|
||||||
//0B100
|
break;
|
||||||
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))
|
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);
|
ushort maxChannelOffset = (ushort)(Speed == SpeedType.SpeedFast ? 400 : 200);
|
||||||
|
|
||||||
|
|||||||
@ -1,355 +0,0 @@
|
|||||||
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;
|
base.IsEnabled = value;
|
||||||
if (value)
|
if (value)
|
||||||
{
|
{
|
||||||
// TODO: 王海, 20160303, 修复 KeyboardController。
|
// TODO: 林俊清, 20160303, 修复 KeyboardController。
|
||||||
//_messenger.Register<KeyDownMessage>(this, OnKeyDown);
|
//_messenger.Register<KeyDownMessage>(this, OnKeyDown);
|
||||||
//_messenger.Register<KeyUpMessage>(this, OnKeyUp);
|
//_messenger.Register<KeyUpMessage>(this, OnKeyUp);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -104,7 +104,7 @@ namespace Plane.CopterManagement
|
|||||||
return Copter.FlyToAsync(lat, lng);
|
return Copter.FlyToAsync(lat, lng);
|
||||||
}
|
}
|
||||||
|
|
||||||
public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
|
public Task FlyToAsync(double lat, double lng, float alt)
|
||||||
{
|
{
|
||||||
return Copter.FlyToAsync(lat, lng, alt);
|
return Copter.FlyToAsync(lat, lng, alt);
|
||||||
}
|
}
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@ -1,317 +1,309 @@
|
|||||||
using System;
|
using System;
|
||||||
using System.Collections.Generic;
|
using static Plane.Protocols.MAVLink;
|
||||||
using static Plane.Protocols.MAVLink;
|
|
||||||
|
namespace Plane.Copters
|
||||||
namespace Plane.Copters
|
{
|
||||||
{
|
public partial class PLCopter : CopterImplSharedPart
|
||||||
|
{
|
||||||
|
private bool _fetchingFirmwareVersion;
|
||||||
public partial class PLCopter : CopterImplSharedPart
|
|
||||||
{
|
private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
|
||||||
|
{
|
||||||
private bool _fetchingFirmwareVersion;
|
IsConnected = _internalCopter.IsConnected;
|
||||||
|
}
|
||||||
private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
|
|
||||||
{
|
private void _internalCopter_GetLogDataEvent(string log)
|
||||||
IsConnected = _internalCopter.IsConnected;
|
{
|
||||||
}
|
//飞机消息日志，后面需要改为记录方式
|
||||||
|
StatusText =Name+":"+log;
|
||||||
private void _internalCopter_GetLogDataEvent(string log)
|
var e = new MessageCreatedEventArgs { Message = StatusText };
|
||||||
{
|
RaiseTextReceived(e);
|
||||||
//飞机消息日志，后面需要改为记录方式
|
}
|
||||||
StatusText =Name+":"+log;
|
|
||||||
var e = new MessageCreatedEventArgs { Message = StatusText };
|
private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
|
||||||
RaiseTextReceived(e);
|
{
|
||||||
}
|
switch (StreamType)
|
||||||
|
{
|
||||||
|
case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
|
||||||
private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
|
{
|
||||||
{
|
SatCount = _internalCopter.satcount;
|
||||||
switch (StreamType)
|
GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
|
||||||
{
|
GpsHdop = _internalCopter.gpshdop;
|
||||||
case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
|
Altitude = _internalCopter.gpsalt;
|
||||||
{
|
Retain = BitConverter.GetBytes(_internalCopter.retain);
|
||||||
SatCount = _internalCopter.satcount;
|
|
||||||
GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
|
Voltage = _internalCopter.battery_voltage;
|
||||||
GpsHdop = _internalCopter.gpshdop;
|
CommModuleMode = (FlightMode)_internalCopter.commModuleMode;
|
||||||
CopterErrorCode =_internalCopter.errorcode;
|
CommModuleVersion = _internalCopter.commModuleVersion;
|
||||||
CopterPreCheckPass = _internalCopter.precheckok;
|
IsUnlocked = _internalCopter.isUnlocked;
|
||||||
CopterErrorString = getcoptererrorstr(_internalCopter.errorcode);
|
//Yaw = _internalCopter.yaw;
|
||||||
Altitude = _internalCopter.gpsalt;
|
Heading = _internalCopter.heading;
|
||||||
Retain = BitConverter.GetBytes(_internalCopter.retain);
|
HeartbeatCount++;
|
||||||
|
|
||||||
Voltage = _internalCopter.battery_voltage;
|
if (SatCount > 8)
|
||||||
CommModuleMode = (FlightMode)_internalCopter.commModuleMode;
|
{
|
||||||
CommModuleVersion = _internalCopter.commModuleVersion;
|
IsGpsAccurate = true;
|
||||||
IsUnlocked = _internalCopter.isUnlocked;
|
RecordLat = _internalCopter.lat;
|
||||||
//Yaw = _internalCopter.yaw;
|
RecordLng = _internalCopter.lng;
|
||||||
Heading = _internalCopter.heading;
|
}
|
||||||
HeartbeatCount++;
|
|
||||||
|
Latitude = RecordLat;
|
||||||
if (SatCount > 8)
|
Longitude = RecordLng;
|
||||||
{
|
UpdateFlightDataIfNeeded();
|
||||||
IsGpsAccurate = true;
|
RaiseLocationChangedIfNeeded();
|
||||||
RecordLat = _internalCopter.lat;
|
|
||||||
RecordLng = _internalCopter.lng;
|
RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
|
||||||
}
|
|
||||||
|
break;
|
||||||
Latitude = RecordLat;
|
}
|
||||||
Longitude = RecordLng;
|
case MAVLINK_MSG_ID_ATTITUDE: //无用
|
||||||
UpdateFlightDataIfNeeded();
|
{
|
||||||
RaiseLocationChangedIfNeeded();
|
Roll = _internalCopter.roll;
|
||||||
|
Pitch = _internalCopter.pitch;
|
||||||
RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
|
Yaw = _internalCopter.yaw;
|
||||||
|
TimebootMs = _internalCopter.timebootms;
|
||||||
break;
|
RaiseAttitudeChanged();
|
||||||
}
|
|
||||||
case MAVLINK_MSG_ID_ATTITUDE: //无用
|
break;
|
||||||
{
|
}
|
||||||
Roll = _internalCopter.roll;
|
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要
|
||||||
Pitch = _internalCopter.pitch;
|
{
|
||||||
Yaw = _internalCopter.yaw;
|
Channel1 = _internalCopter.ch1in;
|
||||||
TimebootMs = _internalCopter.timebootms;
|
Channel2 = _internalCopter.ch2in;
|
||||||
RaiseAttitudeChanged();
|
Channel3 = _internalCopter.ch3in;
|
||||||
|
Channel4 = _internalCopter.ch4in;
|
||||||
break;
|
Channel5 = _internalCopter.ch5in;
|
||||||
}
|
Channel6 = _internalCopter.ch6in;
|
||||||
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要
|
Channel7 = _internalCopter.ch7in;
|
||||||
{
|
Channel8 = _internalCopter.ch8in;
|
||||||
Channel1 = _internalCopter.ch1in;
|
|
||||||
Channel2 = _internalCopter.ch2in;
|
RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);
|
||||||
Channel3 = _internalCopter.ch3in;
|
|
||||||
Channel4 = _internalCopter.ch4in;
|
break;
|
||||||
Channel5 = _internalCopter.ch5in;
|
}
|
||||||
Channel6 = _internalCopter.ch6in;
|
case MAVLINK_MSG_ID_VFR_HUD:
|
||||||
Channel7 = _internalCopter.ch7in;
|
{
|
||||||
Channel8 = _internalCopter.ch8in;
|
Heading = _internalCopter.heading; //有用
|
||||||
|
Altitude = _internalCopter.alt; //有用
|
||||||
RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);
|
AirSpeed = _internalCopter.airspeed; //没用
|
||||||
|
GroundSpeed = _internalCopter.groundspeed; //没用
|
||||||
break;
|
|
||||||
}
|
RaiseAltitudeChangedIfNeeded();
|
||||||
case MAVLINK_MSG_ID_VFR_HUD:
|
|
||||||
{
|
break;
|
||||||
Heading = _internalCopter.heading; //有用
|
}
|
||||||
Altitude = _internalCopter.alt; //有用
|
}
|
||||||
AirSpeed = _internalCopter.airspeed; //没用
|
}
|
||||||
GroundSpeed = _internalCopter.groundspeed; //没用
|
|
||||||
|
private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
|
||||||
RaiseAltitudeChangedIfNeeded();
|
{
|
||||||
|
IsUnlocked = _internalCopter.armed;
|
||||||
break;
|
Mode = (FlightMode)_internalCopter.mode;
|
||||||
}
|
// 林俊清, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
|
||||||
}
|
HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
|
||||||
}
|
++HeartbeatCount;
|
||||||
|
|
||||||
private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
|
IsCheckingConnection = false;
|
||||||
{
|
/*
|
||||||
IsUnlocked = _internalCopter.armed;
|
if (FirmwareVersion == null && !_fetchingFirmwareVersion)
|
||||||
Mode = (FlightMode)_internalCopter.mode;
|
{
|
||||||
// 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
|
try
|
||||||
HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
|
{
|
||||||
++HeartbeatCount;
|
_fetchingFirmwareVersion = true;
|
||||||
|
FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
|
||||||
IsCheckingConnection = false;
|
}
|
||||||
/*
|
catch (TimeoutException)
|
||||||
if (FirmwareVersion == null && !_fetchingFirmwareVersion)
|
{
|
||||||
{
|
// 吞掉。
|
||||||
try
|
}
|
||||||
{
|
finally
|
||||||
_fetchingFirmwareVersion = true;
|
{
|
||||||
FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
|
_fetchingFirmwareVersion = false;
|
||||||
}
|
}
|
||||||
catch (TimeoutException)
|
}
|
||||||
{
|
*/
|
||||||
// 吞掉。
|
RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
|
||||||
}
|
}
|
||||||
finally
|
|
||||||
{
|
private void _internalCopter_ReceiveSensorEvent(object sender)
|
||||||
_fetchingFirmwareVersion = false;
|
{
|
||||||
}
|
RaiseSensorDataReceived(EventArgs.Empty);
|
||||||
}
|
}
|
||||||
*/
|
|
||||||
RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
|
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;
|
||||||
private void _internalCopter_ReceiveSensorEvent(object sender)
|
|
||||||
{
|
if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。
|
||||||
RaiseSensorDataReceived(EventArgs.Empty);
|
{
|
||||||
}
|
CalcBatteryPer();
|
||||||
|
}
|
||||||
private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff)
|
else
|
||||||
{
|
{
|
||||||
Voltage = _internalCopter.battery_voltage / 1000;
|
BatteryPer = _internalCopter.battery_remaining;
|
||||||
|
}
|
||||||
if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。
|
|
||||||
{
|
const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308;
|
||||||
CalcBatteryPer();
|
const int MIN_VERSION_OF_GENERATION2 = 0x3000;
|
||||||
}
|
if (FirmwareVersion != null)
|
||||||
else
|
{
|
||||||
{
|
if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
|
||||||
BatteryPer = _internalCopter.battery_remaining;
|
{
|
||||||
}
|
// 飞控提供了 canTakeOff。
|
||||||
|
IsGpsAccurate = canTakeOff;
|
||||||
const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308;
|
}
|
||||||
const int MIN_VERSION_OF_GENERATION2 = 0x3000;
|
else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
|
||||||
if (FirmwareVersion != null)
|
{
|
||||||
{
|
// 2.0 飞行器，飞控未提供 canTakeOff。
|
||||||
if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
|
IsGpsAccurate = SatCount >= 12;
|
||||||
{
|
}
|
||||||
// 飞控提供了 canTakeOff。
|
else
|
||||||
IsGpsAccurate = canTakeOff;
|
{
|
||||||
}
|
// 1.0 飞行器，飞控不提供 canTakeOff。
|
||||||
else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
|
IsGpsAccurate = SatCount >= 6;
|
||||||
{
|
}
|
||||||
// 2.0 飞行器，飞控未提供 canTakeOff。
|
}
|
||||||
IsGpsAccurate = SatCount >= 12;
|
else
|
||||||
}
|
IsGpsAccurate = SatCount >= 8;
|
||||||
else
|
|
||||||
{
|
FlightDistance2D = _internalCopter.FlightDistance2D;
|
||||||
// 1.0 飞行器，飞控不提供 canTakeOff。
|
|
||||||
IsGpsAccurate = SatCount >= 6;
|
RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
|
||||||
}
|
}
|
||||||
}
|
|
||||||
else
|
private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
|
||||||
IsGpsAccurate = SatCount >= 8;
|
{
|
||||||
|
switch (e.Packet[5])
|
||||||
FlightDistance2D = _internalCopter.FlightDistance2D;
|
{
|
||||||
|
case MAVLINK_MSG_ID_MISSION_COUNT:
|
||||||
RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
|
MissionCount = _internalCopter.WpCount;
|
||||||
}
|
break;
|
||||||
|
|
||||||
private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
|
default:
|
||||||
{
|
break;
|
||||||
switch (e.Packet[5])
|
}
|
||||||
{
|
}
|
||||||
case MAVLINK_MSG_ID_MISSION_COUNT:
|
|
||||||
MissionCount = _internalCopter.WpCount;
|
private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
|
||||||
break;
|
{
|
||||||
|
switch (e.Packet[5])
|
||||||
default:
|
{
|
||||||
break;
|
case MAVLINK_MSG_ID_MISSION_ACK:
|
||||||
}
|
AnalyzeMissionAckPacket(e.Packet);
|
||||||
}
|
break;
|
||||||
|
|
||||||
private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
|
case MAVLINK_MSG_ID_MISSION_COUNT:
|
||||||
{
|
AnalyzeMissionCountPacket(e.Packet);
|
||||||
switch (e.Packet[5])
|
break;
|
||||||
{
|
|
||||||
case MAVLINK_MSG_ID_MISSION_ACK:
|
case MAVLINK_MSG_ID_MISSION_ITEM:
|
||||||
AnalyzeMissionAckPacket(e.Packet);
|
AnalyzeMissionItemPacket(e.Packet);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_MISSION_COUNT:
|
case MAVLINK_MSG_ID_MISSION_REQUEST:
|
||||||
AnalyzeMissionCountPacket(e.Packet);
|
AnalyzeMissionRequestPacket(e.Packet);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_MISSION_ITEM:
|
case MAVLINK_MSG_ID_SET_PAIR:
|
||||||
AnalyzeMissionItemPacket(e.Packet);
|
AnalyzeSetPairPacket(e.Packet);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_MISSION_REQUEST:
|
default:
|
||||||
AnalyzeMissionRequestPacket(e.Packet);
|
break;
|
||||||
break;
|
}
|
||||||
|
}
|
||||||
case MAVLINK_MSG_ID_SET_PAIR:
|
|
||||||
AnalyzeSetPairPacket(e.Packet);
|
#if PRIVATE
|
||||||
break;
|
protected virtual void RegisterInternalCopterEventHandlers()
|
||||||
|
#else
|
||||||
default:
|
|
||||||
break;
|
private void RegisterInternalCopterEventHandlers()
|
||||||
}
|
#endif
|
||||||
}
|
{
|
||||||
|
_internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
|
||||||
#if PRIVATE
|
_internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
|
||||||
protected virtual void RegisterInternalCopterEventHandlers()
|
_internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
|
||||||
#else
|
_internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
|
||||||
|
_internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
|
||||||
private void RegisterInternalCopterEventHandlers()
|
_internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
|
||||||
#endif
|
_internalCopter.PacketHandled += _internalCopter_PacketHandled;
|
||||||
{
|
_internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
|
||||||
_internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
|
_internalCopter.MessageCreated += _internalCopter_MessageCreated;
|
||||||
_internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
|
}
|
||||||
_internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
|
|
||||||
_internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
|
private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e)
|
||||||
_internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
|
{
|
||||||
_internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
|
var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message };
|
||||||
_internalCopter.PacketHandled += _internalCopter_PacketHandled;
|
RaiseTextReceived(txte);
|
||||||
_internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
|
}
|
||||||
_internalCopter.MessageCreated += _internalCopter_MessageCreated;
|
|
||||||
}
|
#region 计算剩余电量
|
||||||
|
|
||||||
private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e)
|
private int bPerTimes;
|
||||||
{
|
private int outBatteryPer;
|
||||||
var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message };
|
private int[] tPerTimes = new int[20];
|
||||||
RaiseTextReceived(txte);
|
private int v_num;
|
||||||
}
|
|
||||||
|
/// <summary>
|
||||||
#region 计算剩余电量
|
/// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
|
||||||
|
/// </summary>
|
||||||
private int bPerTimes;
|
private void CalcBatteryPer()
|
||||||
private int outBatteryPer;
|
{
|
||||||
private int[] tPerTimes = new int[20];
|
float volmax = 0f;
|
||||||
private int v_num;
|
float volmin = 0f;
|
||||||
|
if (Voltage > 5 && Voltage < 9)
|
||||||
/// <summary>
|
{
|
||||||
/// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
|
volmax = 8.2f;
|
||||||
/// </summary>
|
volmin = 7f;
|
||||||
private void CalcBatteryPer()
|
}
|
||||||
{
|
else if (Voltage >= 9
|
||||||
float volmax = 0f;
|
&& Voltage < 13.6)
|
||||||
float volmin = 0f;
|
{
|
||||||
if (Voltage > 5 && Voltage < 9)
|
volmax = 11.6f;
|
||||||
{
|
volmin = 10.2f;
|
||||||
volmax = 8.2f;
|
}
|
||||||
volmin = 7f;
|
else if (Voltage >= 13.6
|
||||||
}
|
&& Voltage < 17.2)
|
||||||
else if (Voltage >= 9
|
{
|
||||||
&& Voltage < 13.6)
|
volmax = 16.3f;
|
||||||
{
|
volmin = 14.2f;
|
||||||
volmax = 11.6f;
|
}
|
||||||
volmin = 10.2f;
|
else if (Voltage >= 17.2
|
||||||
}
|
&& Voltage < 26.2)
|
||||||
else if (Voltage >= 13.6
|
{
|
||||||
&& Voltage < 17.2)
|
volmax = 24.8f;
|
||||||
{
|
volmin = 21.2f;
|
||||||
volmax = 16.3f;
|
}
|
||||||
volmin = 14.2f;
|
|
||||||
}
|
int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));
|
||||||
else if (Voltage >= 17.2
|
|
||||||
&& Voltage < 26.2)
|
if (batteryPer == -1 || volmax == 0 || volmin == 0)
|
||||||
{
|
return;
|
||||||
volmax = 24.8f;
|
|
||||||
volmin = 21.2f;
|
if (bPerTimes < 20)
|
||||||
}
|
{
|
||||||
|
tPerTimes[bPerTimes] = batteryPer;
|
||||||
int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));
|
bPerTimes += 1;
|
||||||
|
}
|
||||||
if (batteryPer == -1 || volmax == 0 || volmin == 0)
|
else
|
||||||
return;
|
{
|
||||||
|
tPerTimes[v_num] = batteryPer;
|
||||||
if (bPerTimes < 20)
|
v_num++;
|
||||||
{
|
if (v_num == 20)
|
||||||
tPerTimes[bPerTimes] = batteryPer;
|
v_num = 0;
|
||||||
bPerTimes += 1;
|
}
|
||||||
}
|
for (int i = 0; i < bPerTimes; i++)
|
||||||
else
|
{
|
||||||
{
|
outBatteryPer += tPerTimes[i];
|
||||||
tPerTimes[v_num] = batteryPer;
|
}
|
||||||
v_num++;
|
|
||||||
if (v_num == 20)
|
outBatteryPer = outBatteryPer / bPerTimes;
|
||||||
v_num = 0;
|
if (outBatteryPer < BatteryPer && bPerTimes > 18)
|
||||||
}
|
{
|
||||||
for (int i = 0; i < bPerTimes; i++)
|
BatteryPer = (byte)outBatteryPer;
|
||||||
{
|
}
|
||||||
outBatteryPer += tPerTimes[i];
|
}
|
||||||
}
|
|
||||||
|
#endregion 计算剩余电量
|
||||||
outBatteryPer = outBatteryPer / bPerTimes;
|
}
|
||||||
if (outBatteryPer < BatteryPer && bPerTimes > 18)
|
}
|
||||||
{
|
|
||||||
BatteryPer = (byte)outBatteryPer;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#endregion 计算剩余电量
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|||||||
@ -144,7 +144,7 @@ namespace Plane.Copters
|
|||||||
return Task.FromResult(true);
|
return Task.FromResult(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
|
protected override Task FlyToCoreAsync(double lat, double lng, float alt)
|
||||||
{
|
{
|
||||||
return TaskUtils.CompletedTask;
|
return TaskUtils.CompletedTask;
|
||||||
}
|
}
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@ -1,86 +1,84 @@
|
|||||||
using System;
|
using System;
|
||||||
|
|
||||||
namespace Plane.Copters
|
namespace Plane.Copters
|
||||||
{
|
{
|
||||||
#if PRIVATE
|
#if PRIVATE
|
||||||
public
|
public
|
||||||
#else
|
#else
|
||||||
internal
|
internal
|
||||||
#endif
|
#endif
|
||||||
enum FlightMode
|
enum FlightMode
|
||||||
{
|
{
|
||||||
// 王海，20150608：不可将以下枚举项重命名。
|
// 林俊清，20150608：不可将以下枚举项重命名。
|
||||||
|
|
||||||
STABILIZE = 0, // hold level position
|
STABILIZE = 0, // hold level position
|
||||||
|
|
||||||
ACRO = 1, // rate control // 王海, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/
|
ACRO = 1, // rate control // 林俊清, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/
|
||||||
|
|
||||||
ALT_HOLD = 2, // AUTO control
|
ALT_HOLD = 2, // AUTO control
|
||||||
|
|
||||||
AUTO = 3, // AUTO control
|
AUTO = 3, // AUTO control
|
||||||
|
|
||||||
GUIDED = 4, // AUTO control
|
GUIDED = 4, // AUTO control
|
||||||
|
|
||||||
LOITER = 5, // Hold a single location
|
LOITER = 5, // Hold a single location
|
||||||
|
|
||||||
RTL = 6, // AUTO control
|
RTL = 6, // AUTO control
|
||||||
|
|
||||||
CIRCLE = 7,
|
CIRCLE = 7,
|
||||||
|
|
||||||
POSITION = 8, // 王海, 20160205, 位置模式：http://copter.ardupilot.cn/wiki/POSITION-mode/
|
POSITION = 8, // 林俊清, 20160205, 位置模式：http://copter.ardupilot.cn/wiki/POSITION-mode/
|
||||||
|
|
||||||
LAND = 9, // AUTO control
|
LAND = 9, // AUTO control
|
||||||
|
|
||||||
OF_LOITER = 10, // 王海, 20160205, 光流悬停模式：http://copter.ardupilot.cn/wiki/loiter-mode/ 底部。
|
OF_LOITER = 10, // 林俊清, 20160205, 光流悬停模式：http://copter.ardupilot.cn/wiki/loiter-mode/ 底部。
|
||||||
|
|
||||||
TOY = 11,
|
TOY = 11
|
||||||
|
}
|
||||||
EMERG_RTL=12, //紧急返航模式
|
|
||||||
}
|
internal static class FightModeExtensions
|
||||||
|
{
|
||||||
internal static class FightModeExtensions
|
public static string GetModeString(this FlightMode flightMode)
|
||||||
{
|
{
|
||||||
public static string GetModeString(this FlightMode flightMode)
|
switch (flightMode)
|
||||||
{
|
{
|
||||||
switch (flightMode)
|
case FlightMode.ALT_HOLD:
|
||||||
{
|
return "ALT HOLD";
|
||||||
case FlightMode.ALT_HOLD:
|
|
||||||
return "ALT HOLD";
|
case FlightMode.POSITION:
|
||||||
|
return "POS HOLD";
|
||||||
case FlightMode.POSITION:
|
|
||||||
return "POS HOLD";
|
default:
|
||||||
|
return Enum.GetName(typeof(FlightMode), flightMode);
|
||||||
default:
|
}
|
||||||
return Enum.GetName(typeof(FlightMode), flightMode);
|
}
|
||||||
}
|
|
||||||
}
|
public static bool NeedGps(this FlightMode flightMode)
|
||||||
|
{
|
||||||
public static bool NeedGps(this FlightMode flightMode)
|
switch (flightMode)
|
||||||
{
|
{
|
||||||
switch (flightMode)
|
case FlightMode.AUTO:
|
||||||
{
|
case FlightMode.GUIDED:
|
||||||
case FlightMode.AUTO:
|
case FlightMode.LOITER:
|
||||||
case FlightMode.GUIDED:
|
case FlightMode.RTL:
|
||||||
case FlightMode.LOITER:
|
case FlightMode.CIRCLE:
|
||||||
case FlightMode.RTL:
|
case FlightMode.LAND:
|
||||||
case FlightMode.CIRCLE:
|
case FlightMode.POSITION:
|
||||||
case FlightMode.LAND:
|
default:
|
||||||
case FlightMode.POSITION:
|
return true;
|
||||||
default:
|
|
||||||
return true;
|
case FlightMode.STABILIZE:
|
||||||
|
case FlightMode.ACRO:
|
||||||
case FlightMode.STABILIZE:
|
case FlightMode.ALT_HOLD:
|
||||||
case FlightMode.ACRO:
|
case FlightMode.OF_LOITER:
|
||||||
case FlightMode.ALT_HOLD:
|
case FlightMode.TOY:
|
||||||
case FlightMode.OF_LOITER:
|
return false;
|
||||||
case FlightMode.TOY:
|
}
|
||||||
return false;
|
}
|
||||||
}
|
|
||||||
}
|
internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode)
|
||||||
|
{
|
||||||
internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode)
|
return (PlaneCopter.ac2modes)flightMode;
|
||||||
{
|
}
|
||||||
return (PlaneCopter.ac2modes)flightMode;
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|||||||
@ -292,7 +292,7 @@ namespace Plane.Copters
|
|||||||
public async Task TakeOffAsync(float alt)
|
public async Task TakeOffAsync(float alt)
|
||||||
{
|
{
|
||||||
var currentTakeOffCount = ++_takeOffCount;
|
var currentTakeOffCount = ++_takeOffCount;
|
||||||
// 王海, 20160312, 从固件版本 3.1.1（0x3101）开始支持直接解锁起飞命令。
|
// 林俊清, 20160312, 从固件版本 3.1.1（0x3101）开始支持直接解锁起飞命令。
|
||||||
//if (FirmwareVersion >= 0x3101)
|
//if (FirmwareVersion >= 0x3101)
|
||||||
{
|
{
|
||||||
await SetChannelsAsync(
|
await SetChannelsAsync(
|
||||||
@ -349,7 +349,7 @@ namespace Plane.Copters
|
|||||||
ch4: 1500
|
ch4: 1500
|
||||||
).ConfigureAwait(false);
|
).ConfigureAwait(false);
|
||||||
|
|
||||||
if (await SetModeAsync(FlightMode.LOITER).ConfigureAwait(false))
|
if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false))
|
||||||
{
|
{
|
||||||
await _internalCopter.DoARMAsync(true).ConfigureAwait(false);
|
await _internalCopter.DoARMAsync(true).ConfigureAwait(false);
|
||||||
}
|
}
|
||||||
@ -418,7 +418,7 @@ namespace Plane.Copters
|
|||||||
return !anotherSetModeActionCalled && Mode == mode;
|
return !anotherSetModeActionCalled && Mode == mode;
|
||||||
}
|
}
|
||||||
|
|
||||||
protected override async Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
|
protected override async Task FlyToCoreAsync(double lat, double lng, float alt)
|
||||||
{
|
{
|
||||||
if (!IsEmergencyHoverActive)
|
if (!IsEmergencyHoverActive)
|
||||||
{
|
{
|
||||||
|
|||||||
@ -46,7 +46,7 @@ namespace Plane.Copters
|
|||||||
((ICollection)bitArray).CopyTo(array, 0);
|
((ICollection)bitArray).CopyTo(array, 0);
|
||||||
return array[0];
|
return array[0];
|
||||||
}
|
}
|
||||||
// 王海，20150703：BitArray 的构造函数的参数是数组长度，这里根本不对啊。
|
// 林俊清，20150703：BitArray 的构造函数的参数是数组长度，这里根本不对啊。
|
||||||
//set
|
//set
|
||||||
//{
|
//{
|
||||||
// bitArray = new BitArray(value);
|
// bitArray = new BitArray(value);
|
||||||
|
|||||||
@ -1,404 +1,402 @@
|
|||||||
//#define LOG_PACKETS
|
//#define LOG_PACKETS
|
||||||
|
|
||||||
using Plane.Protocols;
|
using Plane.Protocols;
|
||||||
using System;
|
using System;
|
||||||
using System.Diagnostics;
|
using System.Diagnostics;
|
||||||
using System.Text;
|
using System.Text;
|
||||||
using System.Threading.Tasks;
|
using System.Threading.Tasks;
|
||||||
using TaskTools.Utilities;
|
using TaskTools.Utilities;
|
||||||
|
|
||||||
#if LOG_PACKETS
|
#if LOG_PACKETS
|
||||||
|
|
||||||
using System.Diagnostics;
|
using System.Diagnostics;
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
namespace Plane.Copters
|
namespace Plane.Copters
|
||||||
{
|
{
|
||||||
partial class PlaneCopter
|
partial class PlaneCopter
|
||||||
{
|
{
|
||||||
private void AnalyzeAttitudePacket(byte[] buffer)
|
private void AnalyzeAttitudePacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var att = buffer.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
|
var att = buffer.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
|
||||||
roll = att.roll * rad2deg;
|
roll = att.roll * rad2deg;
|
||||||
pitch = att.pitch * rad2deg;
|
pitch = att.pitch * rad2deg;
|
||||||
yaw = att.yaw * rad2deg;
|
yaw = att.yaw * rad2deg;
|
||||||
timebootms = att.time_boot_ms;
|
timebootms = att.time_boot_ms;
|
||||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeGlobalPositionIntPacket(byte[] buffer)
|
private void AnalyzeGlobalPositionIntPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var loc = buffer.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
|
var loc = buffer.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
|
||||||
useLocation = true;
|
useLocation = true;
|
||||||
if (loc.lat == 0 && loc.lon == 0)
|
if (loc.lat == 0 && loc.lon == 0)
|
||||||
{
|
{
|
||||||
useLocation = false;
|
useLocation = false;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
gpsalt = loc.alt / 1000.0f;
|
gpsalt = loc.alt / 1000.0f;
|
||||||
lat = loc.lat / 10000000.0;
|
lat = loc.lat / 10000000.0;
|
||||||
lng = loc.lon / 10000000.0;
|
lng = loc.lon / 10000000.0;
|
||||||
}
|
}
|
||||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||||
}
|
}
|
||||||
|
|
||||||
#if LOG_GPS_RAW_INT
|
#if LOG_GPS_RAW_INT
|
||||||
private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch();
|
private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch();
|
||||||
private TimeSpan _lastGpsRawIntPacketTimeSpan;
|
private TimeSpan _lastGpsRawIntPacketTimeSpan;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
private void AnalyzeGpsRawIntPacket(byte[] buffer)
|
private void AnalyzeGpsRawIntPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
#if LOG_GPS_RAW_INT
|
#if LOG_GPS_RAW_INT
|
||||||
if (!_lastGpsRawIntPacketTime.IsRunning)
|
if (!_lastGpsRawIntPacketTime.IsRunning)
|
||||||
{
|
{
|
||||||
_lastGpsRawIntPacketTime.Start();
|
_lastGpsRawIntPacketTime.Start();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
var elapsed = _lastGpsRawIntPacketTime.Elapsed;
|
var elapsed = _lastGpsRawIntPacketTime.Elapsed;
|
||||||
Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt");
|
Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt");
|
||||||
_lastGpsRawIntPacketTimeSpan = elapsed;
|
_lastGpsRawIntPacketTimeSpan = elapsed;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
var gps = buffer.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
|
var gps = buffer.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
|
||||||
long mylat = BitConverter.ToInt32(buffer, 14);
|
long mylat = BitConverter.ToInt32(buffer, 14);
|
||||||
long mylong = BitConverter.ToInt32(buffer, 18);
|
long mylong = BitConverter.ToInt32(buffer, 18);
|
||||||
//if (!useLocation)
|
//if (!useLocation)
|
||||||
//{
|
//{
|
||||||
lat = gps.lat * 1.0e-7;
|
lat = gps.lat * 1.0e-7;
|
||||||
lng = gps.lon * 1.0e-7;
|
lng = gps.lon * 1.0e-7;
|
||||||
//}
|
//}
|
||||||
gpsalt = gps.alt * 1.0e-7f; // using vfr as includes baro calc
|
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
|
gpsstatus = gps.fix_type; //0-1: no fix, 2: 2D fix, 3: 3D fix
|
||||||
gpshdop = (float)Math.Round(gps.eph / 100.0, 2);
|
gpshdop = (float)Math.Round(gps.eph / 100.0, 2);
|
||||||
satcount = gps.satellites_visible;
|
satcount = gps.satellites_visible;
|
||||||
groundspeed = gps.vel * 1.0e-2f;
|
groundspeed = gps.vel * 1.0e-2f;
|
||||||
groundcourse = gps.cog * 1.0e-2f;
|
groundcourse = gps.cog * 1.0e-2f;
|
||||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||||
}
|
}
|
||||||
|
|
||||||
#if LOG_PACKETS
|
#if LOG_PACKETS
|
||||||
private DateTime _lastHeartbeatTime = DateTime.MinValue;
|
private DateTime _lastHeartbeatTime = DateTime.MinValue;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
private void AnalyzeHeartbeatPacket(byte[] buffer)
|
private void AnalyzeHeartbeatPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
#if LOG_PACKETS
|
#if LOG_PACKETS
|
||||||
var now = DateTime.Now;
|
var now = DateTime.Now;
|
||||||
if (_lastHeartbeatTime != DateTime.MinValue)
|
if (_lastHeartbeatTime != DateTime.MinValue)
|
||||||
{
|
{
|
||||||
Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔");
|
Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔");
|
||||||
}
|
}
|
||||||
_lastHeartbeatTime = now;
|
_lastHeartbeatTime = now;
|
||||||
#endif
|
#endif
|
||||||
DataTimeOut = 0;
|
DataTimeOut = 0;
|
||||||
var hb = buffer.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
|
var hb = buffer.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
|
||||||
mavlinkversion = hb.mavlink_version;
|
mavlinkversion = hb.mavlink_version;
|
||||||
aptype = (MAVLink.MAV_TYPE)hb.type;
|
aptype = (MAVLink.MAV_TYPE)hb.type;
|
||||||
apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot;
|
apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot;
|
||||||
sysid = buffer[3];
|
sysid = buffer[3];
|
||||||
compid = buffer[4];
|
compid = buffer[4];
|
||||||
recvpacketcount = buffer[2];
|
recvpacketcount = buffer[2];
|
||||||
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
|
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;
|
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
|
||||||
mode = hb.custom_mode;
|
mode = hb.custom_mode;
|
||||||
|
|
||||||
var handler = ReceiveHeartBearEvent;
|
var handler = ReceiveHeartBearEvent;
|
||||||
if (handler != null)
|
if (handler != null)
|
||||||
{
|
{
|
||||||
RunOnUIThread(() =>
|
RunOnUIThread(() =>
|
||||||
{
|
{
|
||||||
handler(this, iErrorData, iDataCount);
|
handler(this, iErrorData, iDataCount);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_autoRequestData && mavlinkversion == 3 && SendReq == false)
|
if (_autoRequestData && mavlinkversion == 3 && SendReq == false)
|
||||||
{
|
{
|
||||||
SendReq = true;
|
SendReq = true;
|
||||||
Task.Run(GetCopterDataAsync);
|
Task.Run(GetCopterDataAsync);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeMemInfoPacket(byte[] buffer)
|
private void AnalyzeMemInfoPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var mem = buffer.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
|
var mem = buffer.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
|
||||||
freemem = mem.freemem;
|
freemem = mem.freemem;
|
||||||
brklevel = mem.brkval;
|
brklevel = mem.brkval;
|
||||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeMissionCurrentPacket(byte[] buffer)
|
private void AnalyzeMissionCurrentPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var wpcur = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
|
var wpcur = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
|
||||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeNavControllerOutputPacket(byte[] buffer)
|
private void AnalyzeNavControllerOutputPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var nav = buffer.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
|
var nav = buffer.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
|
||||||
nav_roll = nav.nav_roll;
|
nav_roll = nav.nav_roll;
|
||||||
nav_pitch = nav.nav_pitch;
|
nav_pitch = nav.nav_pitch;
|
||||||
nav_bearing = nav.nav_bearing;
|
nav_bearing = nav.nav_bearing;
|
||||||
target_bearing = nav.target_bearing;
|
target_bearing = nav.target_bearing;
|
||||||
wp_dist = nav.wp_dist;
|
wp_dist = nav.wp_dist;
|
||||||
alt_error = nav.alt_error;
|
alt_error = nav.alt_error;
|
||||||
aspd_error = nav.aspd_error / 100.0f;
|
aspd_error = nav.aspd_error / 100.0f;
|
||||||
xtrack_error = nav.xtrack_error;
|
xtrack_error = nav.xtrack_error;
|
||||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeParamValuePacket(byte[] buffer)
|
private void AnalyzeParamValuePacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var par = buffer.ByteArrayToStructure<MAVLink.mavlink_param_value_t>(6);
|
var par = buffer.ByteArrayToStructure<MAVLink.mavlink_param_value_t>(6);
|
||||||
param_total = (par.param_count);
|
param_total = (par.param_count);
|
||||||
string paramID = Encoding.ASCII.GetString(par.param_id);
|
string paramID = Encoding.ASCII.GetString(par.param_id);
|
||||||
int pos = paramID.IndexOf('\0');
|
int pos = paramID.IndexOf('\0');
|
||||||
if (pos != -1)
|
if (pos != -1)
|
||||||
{
|
{
|
||||||
paramID = paramID.Substring(0, pos);
|
paramID = paramID.Substring(0, pos);
|
||||||
}
|
}
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
param[paramID] = (par.param_value);
|
param[paramID] = (par.param_value);
|
||||||
param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type;
|
param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type;
|
||||||
}
|
}
|
||||||
catch (IndexOutOfRangeException ex)
|
catch (IndexOutOfRangeException ex)
|
||||||
{
|
{
|
||||||
Debug.WriteLine(ex);
|
Debug.WriteLine(ex);
|
||||||
}
|
}
|
||||||
|
|
||||||
var handler = ReceiveParamEvent;
|
var handler = ReceiveParamEvent;
|
||||||
if (handler != null)
|
if (handler != null)
|
||||||
{
|
{
|
||||||
RunOnUIThread(() =>
|
RunOnUIThread(() =>
|
||||||
{
|
{
|
||||||
handler(this, paramID, par.param_index, par.param_count);
|
handler(this, paramID, par.param_index, par.param_count);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeRadioPacket(byte[] buffer)
|
private void AnalyzeRadioPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var radio = buffer.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
|
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);
|
//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;
|
remrssi = radio.remrssi;
|
||||||
rssi = radio.rssi;
|
rssi = radio.rssi;
|
||||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeRawImuPacket(byte[] buffer)
|
private void AnalyzeRawImuPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var imu = buffer.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
|
var imu = buffer.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
|
||||||
|
|
||||||
gx = imu.xgyro;
|
gx = imu.xgyro;
|
||||||
gy = imu.ygyro;
|
gy = imu.ygyro;
|
||||||
gz = imu.zgyro;
|
gz = imu.zgyro;
|
||||||
|
|
||||||
ax = imu.xacc;
|
ax = imu.xacc;
|
||||||
ay = imu.yacc;
|
ay = imu.yacc;
|
||||||
az = imu.zacc;
|
az = imu.zacc;
|
||||||
|
|
||||||
mx = imu.xmag;
|
mx = imu.xmag;
|
||||||
my = imu.ymag;
|
my = imu.ymag;
|
||||||
mz = imu.zmag;
|
mz = imu.zmag;
|
||||||
|
|
||||||
var handler = ReceiveSensorEvent;
|
var handler = ReceiveSensorEvent;
|
||||||
if (handler != null)
|
if (handler != null)
|
||||||
{
|
{
|
||||||
RunOnUIThread(() =>
|
RunOnUIThread(() =>
|
||||||
{
|
{
|
||||||
handler(this);
|
handler(this);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeRCChannelsRawPacket(byte[] buffer)
|
private void AnalyzeRCChannelsRawPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var rcin = buffer.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
|
var rcin = buffer.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
|
||||||
|
|
||||||
ch1in = rcin.chan1_raw;
|
ch1in = rcin.chan1_raw;
|
||||||
ch2in = rcin.chan2_raw;
|
ch2in = rcin.chan2_raw;
|
||||||
ch3in = rcin.chan3_raw;
|
ch3in = rcin.chan3_raw;
|
||||||
ch4in = rcin.chan4_raw;
|
ch4in = rcin.chan4_raw;
|
||||||
ch5in = rcin.chan5_raw;
|
ch5in = rcin.chan5_raw;
|
||||||
ch6in = rcin.chan6_raw;
|
ch6in = rcin.chan6_raw;
|
||||||
ch7in = rcin.chan7_raw;
|
ch7in = rcin.chan7_raw;
|
||||||
ch8in = rcin.chan8_raw;
|
ch8in = rcin.chan8_raw;
|
||||||
if (bInitChannel == false)
|
if (bInitChannel == false)
|
||||||
{
|
{
|
||||||
bInitChannel = true;
|
bInitChannel = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
//percent
|
//percent
|
||||||
rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
|
rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
|
||||||
|
|
||||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeScaledImu2Packet(byte[] buffer)
|
private void AnalyzeScaledImu2Packet(byte[] buffer)
|
||||||
{
|
{
|
||||||
var imu2 = buffer.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
|
var imu2 = buffer.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
|
||||||
gx2 = imu2.xgyro;
|
gx2 = imu2.xgyro;
|
||||||
gy2 = imu2.ygyro;
|
gy2 = imu2.ygyro;
|
||||||
gz2 = imu2.zgyro;
|
gz2 = imu2.zgyro;
|
||||||
|
|
||||||
ax2 = imu2.xacc;
|
ax2 = imu2.xacc;
|
||||||
ay2 = imu2.yacc;
|
ay2 = imu2.yacc;
|
||||||
az2 = imu2.zacc;
|
az2 = imu2.zacc;
|
||||||
|
|
||||||
mx2 = imu2.xmag;
|
mx2 = imu2.xmag;
|
||||||
my2 = imu2.ymag;
|
my2 = imu2.ymag;
|
||||||
mz2 = imu2.zmag;
|
mz2 = imu2.zmag;
|
||||||
|
|
||||||
var handler = ReceiveScaleImu2Event;
|
var handler = ReceiveScaleImu2Event;
|
||||||
if (handler != null)
|
if (handler != null)
|
||||||
{
|
{
|
||||||
RunOnUIThread(() => handler(this));
|
RunOnUIThread(() => handler(this));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeSensorOffsetsPacket(byte[] buffer)
|
private void AnalyzeSensorOffsetsPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var sensofs = buffer.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
|
var sensofs = buffer.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
|
||||||
|
|
||||||
mag_ofs_x = sensofs.mag_ofs_x;
|
mag_ofs_x = sensofs.mag_ofs_x;
|
||||||
mag_ofs_y = sensofs.mag_ofs_y;
|
mag_ofs_y = sensofs.mag_ofs_y;
|
||||||
mag_ofs_z = sensofs.mag_ofs_z;
|
mag_ofs_z = sensofs.mag_ofs_z;
|
||||||
mag_declination = sensofs.mag_declination;
|
mag_declination = sensofs.mag_declination;
|
||||||
|
|
||||||
raw_press = sensofs.raw_press;
|
raw_press = sensofs.raw_press;
|
||||||
raw_temp = sensofs.raw_temp;
|
raw_temp = sensofs.raw_temp;
|
||||||
|
|
||||||
gyro_cal_x = sensofs.gyro_cal_x;
|
gyro_cal_x = sensofs.gyro_cal_x;
|
||||||
gyro_cal_y = sensofs.gyro_cal_y;
|
gyro_cal_y = sensofs.gyro_cal_y;
|
||||||
gyro_cal_z = sensofs.gyro_cal_z;
|
gyro_cal_z = sensofs.gyro_cal_z;
|
||||||
|
|
||||||
accel_cal_x = sensofs.accel_cal_x;
|
accel_cal_x = sensofs.accel_cal_x;
|
||||||
accel_cal_y = sensofs.accel_cal_y;
|
accel_cal_y = sensofs.accel_cal_y;
|
||||||
accel_cal_z = sensofs.accel_cal_z;
|
accel_cal_z = sensofs.accel_cal_z;
|
||||||
|
|
||||||
var handler = ReceiveSensorEvent;
|
var handler = ReceiveSensorEvent;
|
||||||
if (handler != null)
|
if (handler != null)
|
||||||
{
|
{
|
||||||
RunOnUIThread(() =>
|
RunOnUIThread(() =>
|
||||||
{
|
{
|
||||||
handler(this);
|
handler(this);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeStatusTextPacket(byte[] buffer)
|
private void AnalyzeStatusTextPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var stext = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
|
var stext = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
|
||||||
var length = Array.IndexOf<byte>(stext.text, 0);
|
var length = Array.IndexOf<byte>(stext.text, 0);
|
||||||
var log = Encoding.ASCII.GetString(stext.text, 0, length);
|
var log = Encoding.ASCII.GetString(stext.text, 0, length);
|
||||||
var handler = GetLogDataEvent;
|
var handler = GetLogDataEvent;
|
||||||
if (handler != null)
|
if (handler != null)
|
||||||
{
|
{
|
||||||
RunOnUIThread(() => handler(log));
|
RunOnUIThread(() => handler(log));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeSysStatusPacket(byte[] buffer)
|
private void AnalyzeSysStatusPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var sysstatus = buffer.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
|
var sysstatus = buffer.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
|
||||||
battery_voltage = sysstatus.voltage_battery;
|
battery_voltage = sysstatus.voltage_battery;
|
||||||
battery_remaining = sysstatus.battery_remaining;
|
battery_remaining = sysstatus.battery_remaining;
|
||||||
current_battery = sysstatus.current_battery / 100.0f;
|
current_battery = sysstatus.current_battery / 100.0f;
|
||||||
|
|
||||||
var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
|
var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
|
||||||
var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
|
var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
|
||||||
|
|
||||||
if (sensors_health.gps != sensors_enabled.gps)
|
if (sensors_health.gps != sensors_enabled.gps)
|
||||||
{
|
{
|
||||||
bGPSBadHealth = true;
|
bGPSBadHealth = true;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
bGPSBadHealth = false;
|
bGPSBadHealth = false;
|
||||||
}
|
}
|
||||||
if (sensors_health.gyro != sensors_enabled.gyro)
|
if (sensors_health.gyro != sensors_enabled.gyro)
|
||||||
{
|
{
|
||||||
bGyroBadHealth = true;
|
bGyroBadHealth = true;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
bGyroBadHealth = false;
|
bGyroBadHealth = false;
|
||||||
}
|
}
|
||||||
if (sensors_health.accelerometer != sensors_enabled.accelerometer)
|
if (sensors_health.accelerometer != sensors_enabled.accelerometer)
|
||||||
{
|
{
|
||||||
bAccel = true;
|
bAccel = true;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
bAccel = false;
|
bAccel = false;
|
||||||
}
|
}
|
||||||
if (sensors_health.compass != sensors_enabled.compass)
|
if (sensors_health.compass != sensors_enabled.compass)
|
||||||
{
|
{
|
||||||
bCompass = true;
|
bCompass = true;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
bCompass = false;
|
bCompass = false;
|
||||||
}
|
}
|
||||||
if (sensors_health.barometer != sensors_enabled.barometer)
|
if (sensors_health.barometer != sensors_enabled.barometer)
|
||||||
{
|
{
|
||||||
bBarometer = true;
|
bBarometer = true;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
bBarometer = false;
|
bBarometer = false;
|
||||||
}
|
}
|
||||||
FlightDistance2D = sysstatus.errors_count2;
|
FlightDistance2D = sysstatus.errors_count2;
|
||||||
var handler = ReceiveSysStatusEvent;
|
var handler = ReceiveSysStatusEvent;
|
||||||
if (handler != null)
|
if (handler != null)
|
||||||
{
|
{
|
||||||
RunOnUIThread(() =>
|
RunOnUIThread(() =>
|
||||||
{
|
{
|
||||||
handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff);
|
handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private void AnalyzeVfrHudPacket(byte[] buffer)
|
private void AnalyzeVfrHudPacket(byte[] buffer)
|
||||||
{
|
{
|
||||||
var vfr = buffer.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
|
var vfr = buffer.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
|
||||||
groundspeed = vfr.groundspeed;
|
groundspeed = vfr.groundspeed;
|
||||||
airspeed = vfr.airspeed;
|
airspeed = vfr.airspeed;
|
||||||
altorigin = vfr.alt; // this might include baro
|
altorigin = vfr.alt; // this might include baro
|
||||||
ch3percent = vfr.throttle;
|
ch3percent = vfr.throttle;
|
||||||
heading = vfr.heading;
|
heading = vfr.heading;
|
||||||
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 飞控返回数据 处理通信模块发过来的28个字节数据
|
/// 处理通信模块发过来的数据
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="buffer"></param>
|
/// <param name="buffer"></param>
|
||||||
public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version)
|
public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version)
|
||||||
{
|
{
|
||||||
var info = buffer.ByteArrayToStructure<MavComm.comm_copter_info>(0);
|
var info = buffer.ByteArrayToStructure<MavComm.comm_copter_info>(0);
|
||||||
lat = info.lat * 1.0e-7;
|
lat = info.lat * 1.0e-7;
|
||||||
lng = info.lng * 1.0e-7;
|
lng = info.lng * 1.0e-7;
|
||||||
commModuleMode = (uint)info.control_mode;
|
commModuleMode = (uint)info.control_mode;
|
||||||
gpsstatus = (byte)info.gps_status;
|
gpsstatus = (byte)info.gps_status;
|
||||||
errorcode= (byte)info.error_code;
|
gpsalt = ((float)info.alt) / 1000;
|
||||||
precheckok = info.rpecheck_fault == 0;
|
satcount = info.gps_num_sats;
|
||||||
gpsalt = ((float)info.alt) / 1000;
|
isUnlocked = info.lock_status == 1;
|
||||||
satcount = info.gps_num_sats;
|
|
||||||
isUnlocked = info.lock_status == 1;
|
battery_voltage = ((float)info.battery_voltage) / 1000;
|
||||||
|
|
||||||
battery_voltage = ((float)info.battery_voltage) / 1000;
|
heading = (short)((info.heading / 100) % 360);
|
||||||
|
|
||||||
heading = (short)((info.heading / 100) % 360);
|
commModuleVersion = version;
|
||||||
|
|
||||||
commModuleVersion = version;
|
retain = info.retain;
|
||||||
|
RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT);
|
||||||
retain = info.retain;
|
}
|
||||||
RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT);
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@ -34,7 +34,6 @@
|
|||||||
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\CopterControllerBase.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\CopterControllerBase.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\CopterControllerManager.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\CopterControllerManager.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\FlyToController.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\FlyToController.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\GuidController.cs" />
|
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\KeyboardController.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)CopterControllers\KeyboardController.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)CopterManagement\CopterFactory.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)CopterManagement\CopterFactory.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)CopterManagement\SingleCopterManager.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_LITTLE_ENDIAN = 1;
|
||||||
public const int MAVLINK_BIG_ENDIAN = 0;
|
public const int MAVLINK_BIG_ENDIAN = 0;
|
||||||
|
|
||||||
//协议头 飞控4.0以后 254 - 240 Mavlink头 FE 改为 F0
|
//飞控4.0以后 254 - 240
|
||||||
//public const byte MAVLINK_STX = 254;
|
//public const byte MAVLINK_STX = 254;
|
||||||
public const byte MAVLINK_STX = 240;
|
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);
|
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_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 };
|
//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>
|
///<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,
|
TAKEOFF = 22,
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// // 王海, 20160331, 来自王海1，据说是传文件用的。
|
/// // 林俊清, 20160331, 来自郭老师，据说是传文件用的。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
TELL_COPTER = 23,
|
TELL_COPTER = 23,
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// // 王海, 20160331, 来自王海1的邮件，作用未知。
|
/// // 林俊清, 20160331, 来自郭老师的邮件，作用未知。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
FILE_TRANS_MODE = 24,
|
FILE_TRANS_MODE = 24,
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// // 王海, 20160331, 来自王海1的邮件，用于 RF 测试。
|
/// // 林俊清, 20160331, 来自郭老师的邮件，用于 RF 测试。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
RF_TEST = 25,
|
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>
|
///<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>
|
/// <summary> Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) </summary>
|
||||||
public byte type;
|
public byte type;
|
||||||
/// <summary> Autopilot type / class. defined in MAV_AUTOPILOT ENUM
|
/// <summary> Autopilot type / class. defined in MAV_AUTOPILOT ENUM
|
||||||
/// // 王海, 20160311, 亿航用作切过 GPS 模式的标志。</summary>
|
/// // 林俊清, 20160311, 亿航用作切过 GPS 模式的标志。</summary>
|
||||||
public byte autopilot;
|
public byte autopilot;
|
||||||
/// <summary> System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h </summary>
|
/// <summary> System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h </summary>
|
||||||
public byte base_mode;
|
public byte base_mode;
|
||||||
|
|||||||
@ -1,187 +1,160 @@
|
|||||||
using System;
|
using System;
|
||||||
using System.Collections.Generic;
|
using System.Collections.Generic;
|
||||||
using System.Runtime.InteropServices;
|
using System.Runtime.InteropServices;
|
||||||
using System.Text;
|
using System.Text;
|
||||||
|
|
||||||
namespace Plane.Protocols
|
namespace Plane.Protocols
|
||||||
{
|
{
|
||||||
/*
|
/*
|
||||||
*
|
*
|
||||||
* 通信模块协议
|
* 通信模块协议
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
public class MavComm
|
public class MavComm
|
||||||
{
|
{
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 发送数据时的数据包头
|
/// 发送数据时的数据包头
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const ushort COMM_SEND_PACKET_HEADER = 0x9551;
|
public const ushort COMM_SEND_PACKET_HEADER = 0x9551;
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 接受数据时的数据包头
|
/// 接受数据时的数据包头
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713;
|
public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713;
|
||||||
|
|
||||||
|
|
||||||
#region 命令类型 2字节
|
#region 命令类型 2字节
|
||||||
//-----------------命令类型-----------------
|
//-----------------命令类型-----------------
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 查询状态
|
/// 查询状态
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const short COMM_QUERY_STATUS = 0x00;
|
public const short COMM_QUERY_STATUS = 0x00;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 变更飞机总数及参与者
|
/// 变更飞机总数及参与者
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const short COMM_SET_MAV_COUNT = 0x10;
|
public const short COMM_SET_MAV_COUNT = 0x10;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 获取飞机详细信息
|
/// 获取飞机详细信息
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const short COMM_GET_COPTERS_COUNT = 0x20;
|
public const short COMM_GET_COPTERS_COUNT = 0x20;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 时间同步，不改变当前模式
|
/// 时间同步，不改变当前模式
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const short COMM_ASYN_TIME = 0x30;
|
public const short COMM_ASYN_TIME = 0x30;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 进入空闲模式
|
/// 进入空闲模式
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const short COMM_IDLE_MODE = 0x50;
|
public const short COMM_IDLE_MODE = 0x50;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 进入搜索模式，命名编号targetID memcpy(pdata,input,2);
|
/// 进入搜索模式，命名编号targetID memcpy(pdata,input,2);
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const short COMM_SEARCH_MODE = 0x51;
|
public const short COMM_SEARCH_MODE = 0x51;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 进入航点下载模式 (写航点)
|
/// 进入航点下载模式 (写航点)
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const short COMM_DOWNLOAD_MODE = 0x52;
|
public const short COMM_DOWNLOAD_MODE = 0x52;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 下载模式通信
|
/// 下载模式通信
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const short COMM_DOWNLOAD_COMM = 0x53;
|
public const short COMM_DOWNLOAD_COMM = 0x53;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 进入飞行模式（无负载）
|
/// 进入飞行模式（无负载）
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const short COMM_FLIGHT_MODE = 0x54;
|
public const short COMM_FLIGHT_MODE = 0x54;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 进入飞行模式（有负载数据）
|
/// 进入飞行模式（有负载数据）
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public const short COMM_FLIGHT_LOAD_MODE = 0x55;
|
public const short COMM_FLIGHT_LOAD_MODE = 0x55;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
/// <summary>
|
/// 通信模块
|
||||||
/// 发送非针对飞控的内部控制指令
|
/// </summary>
|
||||||
/// </summary>
|
public const short COMM_NOTIFICATION = 0x1234;
|
||||||
public const short COMM_TO_MODULE = 0x5F;
|
|
||||||
|
/// <summary>
|
||||||
|
/// 空中升级(更新通信模块飞机端)
|
||||||
|
/// </summary>
|
||||||
/// <summary>
|
public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
|
||||||
/// 通信模块
|
|
||||||
/// </summary>
|
#endregion
|
||||||
public const short COMM_NOTIFICATION = 0x1234;
|
|
||||||
|
public enum CommMode
|
||||||
/// <summary>
|
{
|
||||||
/// 空中升级(更新通信模块飞机端)
|
IDLE = 0,
|
||||||
/// </summary>
|
SEARCH = 1,
|
||||||
public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
|
DOWNLOAD = 3,
|
||||||
|
FLIGHT = 4
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
public enum MessageType
|
||||||
/// 测试模块
|
{
|
||||||
/// </summary>
|
STR = 0,
|
||||||
public const short COMM_TEST_MODULE = 0x3C;
|
SCAN2 = 2,
|
||||||
|
SCAN3 = 3
|
||||||
|
}
|
||||||
|
|
||||||
#endregion
|
//search 搜索模式相关
|
||||||
|
public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功
|
||||||
public enum CommMode
|
public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功
|
||||||
{
|
public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标
|
||||||
IDLE = 0,
|
public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误
|
||||||
SEARCH = 1,
|
public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S
|
||||||
DOWNLOAD = 3,
|
|
||||||
FLIGHT = 4
|
|
||||||
}
|
public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息,
|
||||||
|
//需要再后续等待一个成功消息，才算完成
|
||||||
public enum MessageType
|
|
||||||
{
|
public const ushort P2P_SEND_FAILED = 0x0600; //点对点通信发送失败
|
||||||
STR = 0,
|
|
||||||
SCAN2 = 2,
|
public const ushort ERROR_CODE_START = 0x0100;
|
||||||
SCAN3 = 3
|
public const ushort ERROR_CODE_END = 0x03FF;
|
||||||
}
|
|
||||||
|
|
||||||
//search 搜索模式相关
|
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
|
||||||
public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功
|
public struct comm_set_mav_count
|
||||||
public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功
|
{
|
||||||
public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标
|
public Int16 mav_count; //飞机总数
|
||||||
public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误
|
};
|
||||||
public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S
|
|
||||||
|
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
|
||||||
|
public struct comm_update_copter_module
|
||||||
public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息,
|
{
|
||||||
//需要再后续等待一个成功消息，才算完成
|
public Int16 mav_count; //飞机总数
|
||||||
|
public Int16 update_code;
|
||||||
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 = 8)]
|
||||||
|
public struct comm_asyn_time
|
||||||
|
{
|
||||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
|
public Int64 time_stamp;
|
||||||
public struct comm_set_mav_count
|
};
|
||||||
{
|
|
||||||
public Int16 mav_count; //飞机总数
|
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
|
||||||
};
|
public struct comm_copter_info
|
||||||
|
{
|
||||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
|
public Int32 control_mode;
|
||||||
public struct comm_update_copter_module
|
public UInt32 lat;
|
||||||
{
|
public UInt32 lng;
|
||||||
public Int16 mav_count; //飞机总数
|
public float retain;
|
||||||
public Int16 update_code;
|
public Int32 alt;
|
||||||
};
|
|
||||||
|
public Int16 gps_status;
|
||||||
|
|
||||||
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
|
public byte lock_status;
|
||||||
public struct comm_asyn_time
|
public byte gps_num_sats;
|
||||||
{
|
public Int16 battery_voltage;
|
||||||
public Int64 time_stamp;
|
public Int16 heading;
|
||||||
};
|
|
||||||
|
};
|
||||||
[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);
|
Marshal.Copy(bytearray, startoffset, i, len);
|
||||||
packet = Marshal.PtrToStructure<TMavlinkPacket>(i);
|
packet = Marshal.PtrToStructure<TMavlinkPacket>(i);
|
||||||
}
|
}
|
||||||
// 王海, 20151026, 改为不吞异常了。
|
// 林俊清, 20151026, 改为不吞异常了。
|
||||||
//catch
|
//catch
|
||||||
//{
|
//{
|
||||||
// //log.Error("ByteArrayToStructure FAIL", ex);
|
// //log.Error("ByteArrayToStructure FAIL", ex);
|
||||||
|
|||||||
@ -43,7 +43,7 @@ namespace Plane.Protocols
|
|||||||
Marshal.Copy(bytearray, startoffset, i, len);
|
Marshal.Copy(bytearray, startoffset, i, len);
|
||||||
packet = MarshalEx.PtrToStructure<TMavlinkPacket>(i);
|
packet = MarshalEx.PtrToStructure<TMavlinkPacket>(i);
|
||||||
}
|
}
|
||||||
// 王海, 20151026, 改为不吞异常了。
|
// 林俊清, 20151026, 改为不吞异常了。
|
||||||
//catch
|
//catch
|
||||||
//{
|
//{
|
||||||
// //log.Error("ByteArrayToStructure FAIL", ex);
|
// //log.Error("ByteArrayToStructure FAIL", ex);
|
||||||
|
|||||||
@ -39,13 +39,13 @@ namespace TaskTools.HIL
|
|||||||
self.z);
|
self.z);
|
||||||
}
|
}
|
||||||
|
|
||||||
// 王海，20150702：删除 MissionPlanner.Utilities.dll。
|
// 林俊清，20150702：删除 MissionPlanner.Utilities.dll。
|
||||||
//public static implicit operator Vector3(PointLatLngAlt a)
|
//public static implicit operator Vector3(PointLatLngAlt a)
|
||||||
//{
|
//{
|
||||||
// return new Vector3(a.Lat,a.Lng,a.Alt);
|
// 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)
|
//public static implicit operator Vector3(PointLatLng a)
|
||||||
//{
|
//{
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user