group/Plane.Sdk3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

merge into: group:Xu-V2.0
Branches Tags
group:Dev
group:release_2.1
group:发布兼容版5.28
group:兼容老板_1
group:Xu-V2.0
group:兼容4.1.2以前的版本
group:Branch_noflytobug
group:master
...
pull from: group:Dev
Branches Tags
group:Dev
group:release_2.1
group:发布兼容版5.28
group:兼容老板_1
group:Xu-V2.0
group:兼容4.1.2以前的版本
group:Branch_noflytobug
group:master

23 Commits
Xu-V2.0 ... Dev

Author SHA1 Message Date
xu
8af733e644 [fix] 退出异常调整测试灯光 
详细描述

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
 2024-07-02 15:00:29 +08:00
xu
ffb95b2e7d [feat] 增加广播通道发送关键指令 
详细描述

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
 2024-06-30 11:15:19 +08:00
xu
361a8bf001 [feat] 调整广播发送函数 
为所有需要广播的地方调用做准备

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
 2024-06-29 20:30:28 +08:00
xu
40bf208054 [fix] 广播端口改为异步发送 
测试发现RTK有阻塞，导致死机

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
 2024-06-28 23:15:26 +08:00
xu
a88311a160 [feat] 调整广播发送方式 
调整广播发送和通讯模块发送一样的数据为了兼容有模块和没模块的，也可以双通道同时发送提高可靠性
# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
 2024-06-28 18:33:12 +08:00
xu
e909b7a3d3 [类型] 加入更多错误提示和rtk重发 
rtk重发后还是更可靠一些，并且不会导致数量大阻塞

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
 2024-06-24 20:44:12 +08:00
xu
3e3b6f08ee [fix] 兼容老固件 
没有改完，兼容老固件的rtcm需要
         1.gps.target_system =1
         2.不能多次发送
目前1还没做到，因为库里没法判断是新的还是老的

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
 2024-06-24 15:39:41 +08:00
xu
2045f87a83 RTCM广播采用和通讯模块一致的MAVLINK_MSG_ID_GPS_INJECT_DATA消息，飞控响应一致 2024-04-27 15:35:19 +08:00
xu
f82a285f8d 新换电脑安装最新vs2022后框架更新为4.8 
drawing引用更新为4.8的
 2024-04-08 19:40:01 +08:00
pxzleo
f0a4484cfc 支持紧急返航 
支持返回飞机错误
 2024-03-26 21:58:48 +08:00
pxzleo
9c2238479f 紧急返航功能，策略不对，临时保存 2024-01-17 22:37:53 +08:00
pxzleo
f9814532f5 去掉重连提示，要不日志里面太多了 2024-01-08 20:40:10 +08:00
pxzleo
c0f0f616dc 更新使用Vs2022编译 
模拟飞机路线计算调整
 2023-12-25 18:54:28 +08:00
pxzleo
14c489c016 小航点计算飞行路径,计算4种情况，匀速。加速匀速减速，加速匀速，匀速减速 2023-12-04 23:48:14 +08:00
pxzleo
d051300171 1增加观测者位置图标，修改后直接可以使用 
2增加配置FC_VER_NO = 3 使用小航点同时达到模式计算模拟飞行
3增加灯光框架，可实现闪烁等，但实际使用太慢，暂时不用
 2023-11-21 21:18:03 +08:00
pxzleo
fc9b2595d6 地面站硬件超时逻辑修改为2秒收不到再提示 
起飞前改到loiter模式
 2023-11-16 23:37:57 +08:00
pxzleo
14d1022775 1加入RTCM数据广播到指定端口功能 
2临时修改协议头到通用的FE，后面要改回来
 2023-10-19 22:14:37 +08:00
pxzleo
c72b6273b6 加入RTK广播端口 2023-10-15 11:08:32 +08:00
pxzleo
f175def6a7 通讯检测 2023-05-01 13:57:32 +08:00
xu
8f2cd98bd4 1.加入回传功率设置 
2.服务器IP可从设置文件读取
3.增加抛物功能
 2021-12-27 12:41:32 +08:00
xu
d91b759d5f 1.增加测试某个通讯模块是否正常功能 
2.增加电池电压测试
3.自动计算模拟飞机更新时间300内保证流程，1000内保证能工作
 2021-04-02 01:18:56 +08:00
xu
f7be769b33 改回50间隔 
修复方向不正确的bug
 2020-11-26 18:51:13 +08:00
xu
3e46bbb376 修改名字 2020-09-20 11:43:27 +08:00
37 changed files with 8584 additions and 7115 deletions
Show Stats Expand all files Collapse all files

2
PlaneGcsSdk.Contract_Private/Copters/ICopterActions.cs
View File

@ -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>

8
PlaneGcsSdk.Contract_Private/PlaneGcsSdk.Contract_Private.csproj
View File

@ -34,15 +34,17 @@
<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.

5
PlaneGcsSdk.Contract_Shared/Copters/Constants.cs
View File

@ -23,5 +23,10 @@
/// 最高速度，单位为 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; //下降速度
} }
} }

4
PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs
View File

@ -31,7 +31,9 @@ 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>
Task FlyToAsync(double lat, double lng, float alt); /// flytime =飞行时间 秒
/// flytype 0;匀速 1;加减速 2;单加速 3;单减速
Task FlyToAsync(double lat, double lng, float alt,float flytime=0,int flytype = 0);
/// <summary> /// <summary>
/// 切换到 <see cref="CopterState.HoverMode"/> 并悬停。此操作需要使用 GPS 定位，卫星数不足导致定位不准时非常危险。 /// 切换到 <see cref="CopterState.HoverMode"/> 并悬停。此操作需要使用 GPS 定位，卫星数不足导致定位不准时非常危险。

8
PlaneGcsSdk.Contract_Shared/Copters/ICopterAttribute.cs
View File

@ -14,5 +14,13 @@ 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; }
} }
} }

15
PlaneGcsSdk.Contract_Shared/Copters/ICopterStatus.cs
View File

@ -1,5 +1,6 @@
using Plane.Geography; using Plane.Geography;
using System; using System;
using System.Drawing;
namespace Plane.Copters namespace Plane.Copters
{ {
@ -187,5 +188,19 @@ 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; }
} }
} }

2
PlaneGcsSdk.Contract_Shared/Copters/IFakeCopter.cs
View File

@ -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,

39
PlaneGcsSdk.Contract_Shared/Geography/GeographyUtils.cs
View File

@ -55,6 +55,45 @@ 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>
/// 计算空间中两点之间的距离，单位为米。 /// 计算空间中两点之间的距离，单位为米。

2
PlaneGcsSdk_Private_NET45/Copters/FakeCopter.Private.cs
View File

@ -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>

2
PlaneGcsSdk_Private_NET45/Copters/PLCopter.Private.cs
View File

@ -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>

2
PlaneGcsSdk_Private_NET45/Copters/PlaneCopter.Private.cs
View File

@ -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>

158
PlaneGcsSdk_Private_NET46/PlaneGcsSdk_Private_NET46.csproj
View File

@ -1,79 +1,81 @@
<?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.6</TargetFrameworkVersion> <TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
<FileAlignment>512</FileAlignment> <FileAlignment>512</FileAlignment>
</PropertyGroup> <TargetFrameworkProfile />
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' "> </PropertyGroup>
<DebugSymbols>true</DebugSymbols> <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
<DebugType>full</DebugType> <DebugSymbols>true</DebugSymbols>
<Optimize>false</Optimize> <DebugType>full</DebugType>
<OutputPath>bin\Debug\</OutputPath> <Optimize>false</Optimize>
<DefineConstants>TRACE;DEBUG;PRIVATE</DefineConstants> <OutputPath>bin\Debug\</OutputPath>
<ErrorReport>prompt</ErrorReport> <DefineConstants>TRACE;DEBUG;PRIVATE</DefineConstants>
<WarningLevel>4</WarningLevel> <ErrorReport>prompt</ErrorReport>
</PropertyGroup> <WarningLevel>4</WarningLevel>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' "> </PropertyGroup>
<DebugType>pdbonly</DebugType> <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
<Optimize>true</Optimize> <DebugType>pdbonly</DebugType>
<OutputPath>bin\Release\</OutputPath> <Optimize>true</Optimize>
<DefineConstants>TRACE;PRIVATE</DefineConstants> <OutputPath>bin\Release\</OutputPath>
<ErrorReport>prompt</ErrorReport> <DefineConstants>TRACE;PRIVATE</DefineConstants>
<WarningLevel>4</WarningLevel> <ErrorReport>prompt</ErrorReport>
<DocumentationFile>bin\Release\PlaneGcsSdk_Private_NET46.xml</DocumentationFile> <WarningLevel>4</WarningLevel>
</PropertyGroup> <DocumentationFile>bin\Release\PlaneGcsSdk_Private_NET46.xml</DocumentationFile>
<ItemGroup> </PropertyGroup>
<Reference Include="PresentationCore" /> <ItemGroup>
<Reference Include="System" /> <Reference Include="PresentationCore" />
<Reference Include="System.Core" /> <Reference Include="System" />
<Reference Include="System.Xml.Linq" /> <Reference Include="System.Core" />
<Reference Include="System.Data.DataSetExtensions" /> <Reference Include="System.Drawing" />
<Reference Include="Microsoft.CSharp" /> <Reference Include="System.Xml.Linq" />
<Reference Include="System.Data" /> <Reference Include="System.Data.DataSetExtensions" />
<Reference Include="System.Net.Http" /> <Reference Include="Microsoft.CSharp" />
<Reference Include="System.Xml" /> <Reference Include="System.Data" />
<Reference Include="WindowsBase" /> <Reference Include="System.Net.Http" />
</ItemGroup> <Reference Include="System.Xml" />
<ItemGroup> <Reference Include="WindowsBase" />
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\CopterImplSharedPart.Private.cs"> </ItemGroup>
<Link>Copters\CopterImplSharedPart.Private.cs</Link> <ItemGroup>
</Compile> <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\CopterImplSharedPart.Private.cs">
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PlaneCopter.Private.cs"> <Link>Copters\CopterImplSharedPart.Private.cs</Link>
<Link>Copters\PlaneCopter.Private.cs</Link> </Compile>
</Compile> <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PlaneCopter.Private.cs">
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PLCopter.Private.cs"> <Link>Copters\PlaneCopter.Private.cs</Link>
<Link>Copters\EHCopter.Private.cs</Link> </Compile>
</Compile> <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\PLCopter.Private.cs">
<Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\FakeCopter.Private.cs"> <Link>Copters\EHCopter.Private.cs</Link>
<Link>Copters\FakeCopter.Private.cs</Link> </Compile>
</Compile> <Compile Include="..\PlaneGcsSdk_Private_NET45\Copters\FakeCopter.Private.cs">
<Compile Include="Properties\AssemblyInfo.cs" /> <Link>Copters\FakeCopter.Private.cs</Link>
</ItemGroup> </Compile>
<ItemGroup> <Compile Include="Properties\AssemblyInfo.cs" />
<ProjectReference Include="..\..\Plane.Libraries\Plane.Windows.Messages\Plane.Windows.Messages.csproj"> </ItemGroup>
<Project>{413C18E2-235F-4E15-B5C1-633657DE5D7A}</Project> <ItemGroup>
<Name>Plane.Windows.Messages</Name> <ProjectReference Include="..\..\Plane.Libraries\Plane.Windows.Messages\Plane.Windows.Messages.csproj">
</ProjectReference> <Project>{413C18E2-235F-4E15-B5C1-633657DE5D7A}</Project>
<ProjectReference Include="..\PlaneGcsSdk.Contract_Private\PlaneGcsSdk.Contract_Private.csproj"> <Name>Plane.Windows.Messages</Name>
<Project>{47141894-ece3-48ca-8dcf-ca751bda231e}</Project> </ProjectReference>
<Name>PlaneGcsSdk.Contract_Private</Name> <ProjectReference Include="..\PlaneGcsSdk.Contract_Private\PlaneGcsSdk.Contract_Private.csproj">
</ProjectReference> <Project>{47141894-ece3-48ca-8dcf-ca751bda231e}</Project>
</ItemGroup> <Name>PlaneGcsSdk.Contract_Private</Name>
<Import Project="..\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.projitems" Label="Shared" /> </ProjectReference>
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" /> </ItemGroup>
<!-- To modify your build process, add your task inside one of the targets below and uncomment it. <Import Project="..\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.projitems" Label="Shared" />
Other similar extension points exist, see Microsoft.Common.targets. <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
<Target Name="BeforeBuild"> <!-- To modify your build process, add your task inside one of the targets below and uncomment it.
</Target> Other similar extension points exist, see Microsoft.Common.targets.
<Target Name="AfterBuild"> <Target Name="BeforeBuild">
</Target> </Target>
--> <Target Name="AfterBuild">
</Target>
-->
</Project> </Project>

254
PlaneGcsSdk_Shared/Communication/TcpConnection.cs
View File

@ -1,116 +1,138 @@
#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,
private int _remotePort; const int tcpReceiveTimeout = 1200;
public TcpConnection(string remoteHostname, int remotePort = 5250) private int _remotePort;
{
_remoteHostname = remoteHostname; public TcpConnection(string remoteHostname, int remotePort = 5250)
_remotePort = remotePort; {
_client = new TcpClient _remoteHostname = remoteHostname;
{ _remotePort = remotePort;
ReceiveBufferSize = 40 * 1024, _client = new TcpClient
ReceiveTimeout = 1200 {
}; ReceiveBufferSize = tcpReceiveBufferSize,
} ReceiveTimeout = tcpReceiveTimeout
};
public void Open() }
{
if (!IsOpen) public void Open()
{ {
try if (!IsOpen)
{ {
_client.Connect(_remoteHostname, _remotePort); try
} {
catch (SocketException) _client.Connect(_remoteHostname, _remotePort);
{ }
CreateClientAndConnect(); catch (SocketException)
} {
catch (ObjectDisposedException) CreateClientAndConnect();
{ }
CreateClientAndConnect(); catch (ObjectDisposedException)
} {
_isBroken = false; CreateClientAndConnect();
} }
_isBroken = false;
_stream = _client.GetStream(); }
}
_stream = _client.GetStream();
public async Task<bool> BindIP(string ip) }
{
bool bind = false; public async Task<bool> BindIP(string ip)
try {
{ bool bind = false;
IPAddress IPLocal = IPAddress.Parse(ip); try
_client.Client.Bind(new IPEndPoint(IPLocal, 0)); {
bind = true; IPAddress IPLocal = IPAddress.Parse(ip);
} _client.Client.Bind(new IPEndPoint(IPLocal, 0));
catch bind = true;
{ }
bind = false; catch
} {
await Task.Delay(10).ConfigureAwait(false); bind = false;
return bind; }
} await Task.Delay(10).ConfigureAwait(false);
return bind;
public override async Task OpenAsync() }
{
string logstr; public override async Task OpenAsync()
if (!IsOpen) {
{ string logstr;
try if (!IsOpen)
{ {
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false); if (_client == null)
} CreateClientAndConnect();
//屏蔽掉异常处理 try
//CreateClientAndConnectAsync会new一个TcpClient并且重新连接 {
//之前设置的TcpClient中Socket Bind会无效，在多网卡工作时会导致断线重连的时间特别长 var connectTask = _client.ConnectAsync(_remoteHostname, _remotePort);
catch (SocketException e) if (await Task.WhenAny(connectTask, Task.Delay(5000)) == connectTask)
{ {
logstr= e.Message; if (_client.Client != null) //需要测试
//await CreateClientAndConnectAsync(); await connectTask.ConfigureAwait(false);
} }
catch (ObjectDisposedException) else
{ {
//await CreateClientAndConnectAsync(); // Connection timed out
} throw new TimeoutException("Connection timed out");
_isBroken = false; }
} }
_stream = _client.GetStream(); catch (SocketException e)
} {
logstr = e.Message;
private void CreateClientAndConnect() CloseClient(); // 关闭并清理客户端
{ }
_client = new TcpClient(_remoteHostname, _remotePort) catch (ObjectDisposedException)
{ {
ReceiveBufferSize = 40 * 1024, CloseClient(); // 关闭并清理客户端
ReceiveTimeout = 1200 }
}; catch (Exception)
} {
CloseClient(); // 处理其他可能的异常
private async Task CreateClientAndConnectAsync() // throw; // 可选：重新抛出异常，以便调用者知道发生了错误
{ }
_client = new TcpClient _isBroken = false;
{ }
ReceiveBufferSize = 40 * 1024, if (_client != null)
ReceiveTimeout = 1200 _stream = _client.GetStream();
}; }
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false); private void CloseClient()
} {
} _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

5
PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs
View File

@ -43,7 +43,10 @@ namespace Plane.Communication
{ {
try try
{ {
return _client.Client != null && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected); // bool bret ;
// bret = _client.Client != null;
// bret = bret && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
return _client != null && _client.Client != null && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
} }
catch (ObjectDisposedException) catch (ObjectDisposedException)
{ {

2
PlaneGcsSdk_Shared/Communication/TcpServerConnectionManager.cs
View File

@ -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;

1623
PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
View File

File diff suppressed because it is too large Load Diff

2833
PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs
View File

File diff suppressed because it is too large Load Diff

418
PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs
View File

@ -1,205 +1,213 @@
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 0x0e: case 0x04:
if (copterId == 0) Message.Show($"飞机{copterId}:功率设置成功，新回传功率：{buffer[1]}");
Message.Show("----------全部更新完成----------"); break;
else
Message.Show($"飞机{copterId}:更新进度{buffer[1]}%"); case 0x0e:
break; if (copterId == 0)
} Message.Show("----------全部更新完成----------");
} else
Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
private void SaveMissionWriteStat(short copterId, byte wirteMissRet) break;
{
Task.Run(async () => default:
{ // Message.Show($"Comm3返回：{type}");
lock (MissinLocker) break;
{ }
// var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]); }
if (wirteMissRet == 0x20) private void SaveMissionWriteStat(short copterId, byte wirteMissRet)
{ {
if (missionWriteState.ContainsKey(copterId)) Task.Run(async () =>
missionWriteState[copterId].WriteSucceed = true; {
Message.Show($"ID:{copterId}:航点写入成功"); lock (MissinLocker)
} {
if (wirteMissRet > 0x20 && wirteMissRet < 0x30) // var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]);
{
if (missionWriteState.ContainsKey(copterId)) if (wirteMissRet == 0x20)
missionWriteState[copterId].WriteSucceed = false; {
Message.Show($"ID:{copterId}:航点写入失败"); if (missionWriteState.ContainsKey(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) }
{ }
ushort beginNum = BitConverter.ToUInt16(buffer, 0); await Task.Delay(10).ConfigureAwait(false);
ushort endNum = BitConverter.ToUInt16(buffer, 2); }
int count = endNum - beginNum + 1; );
byte[] copter_packets = buffer.Skip(4).ToArray();
if (copter_packets.Length != count * 4) }
{
return; private void AnalyzeCopterInfosPacket(byte[] buffer)
} {
ushort beginNum = BitConverter.ToUInt16(buffer, 0);
int offset = 0; ushort endNum = BitConverter.ToUInt16(buffer, 2);
for (int i = beginNum; i <= endNum; i++) int count = endNum - beginNum + 1;
{ byte[] copter_packets = buffer.Skip(4).ToArray();
byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray(); if (copter_packets.Length != count * 4)
UInt16 state = BitConverter.ToUInt16(onePacket, 0); {
short copterId = (short)i; return;
switch (state >> 13) }
{
//0B000 int offset = 0;
case 0x0000 >> 13: for (int i = beginNum; i <= endNum; i++)
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); {
break; byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray();
//0B100 UInt16 state = BitConverter.ToUInt16(onePacket, 0);
case 0x8000 >> 13: short copterId = (short)i;
break; switch (state >> 13)
//0B110 {
case 0xC000 >> 13: //0B000
//0B111 case 0x0000 >> 13:
case 0xE000 >> 13: CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId)); break;
break; //0B100
} case 0x8000 >> 13:
offset += 4; break;
} //0B110
} case 0xC000 >> 13:
//0B111
private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer) case 0xE000 >> 13:
{ CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
byte[] packet = buffer.Take(28).ToArray(); break;
byte[] state_packet = buffer.Skip(28).ToArray(); }
UInt16 state = BitConverter.ToUInt16(state_packet,0); offset += 4;
byte version = state_packet[3]; }
switch (state >> 13) }
{
//0B000 private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer)
case 0x0000 >> 13: {
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); byte[] packet = buffer.Take(28).ToArray();
break; byte[] state_packet = buffer.Skip(28).ToArray();
//0B100 UInt16 state = BitConverter.ToUInt16(state_packet,0);
case 0x8000 >> 13: byte version = state_packet[3];
break; switch (state >> 13)
//0B110 {
case 0xC000 >> 13: //0B000
//0B111 case 0x0000 >> 13:
case 0xE000 >> 13: CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version)); break;
break; //0B100
} case 0x8000 >> 13:
break;
} //0B110
} case 0xC000 >> 13:
} //0B111
case 0xE000 >> 13: //最后正在用的版本
CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version));
break;
}
}
}
}

2
PlaneGcsSdk_Shared/CopterControllers/AvatarController.cs
View File

@ -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);

355
PlaneGcsSdk_Shared/CopterControllers/GuidController.cs Normal file
View File

@ -0,0 +1,355 @@
using System;
using System.Collections.Generic;
using System.Text;
namespace Plane.CopterControllers
{
class GuidController
{
float fabsf(float vvalue)
{
return Math.Abs(vvalue);
}
float sqrt(float vvalue)
{
return (float)Math.Sqrt(vvalue);
}
/// <summary>
/// 计算小航点飞行
/// </summary>
/// <param name="Distance"></param>
/// <param name="fc_acc"></param>
/// <param name="fc_maxspeed"></param>
/// <returns></returns>
//单算减速，不算加速的最小飞行时间
float getMinfligthtime_v2(float Distance, float fc_acc, float fc_maxspeed)
{
float fDis = fabsf(Distance); // 总距离
float facc = fabsf(fc_acc); // 减速度
// 物体开始时即以最大速度运动，不考虑加速过程
float vel = fc_maxspeed;
// 计算减速所需的时间和距离
// 减速时间 (从最大速度减速到0)
float dectime = vel / facc;
// 减速阶段覆盖的距离
float decdis = (vel * dectime) - (0.5f * facc * dectime * dectime);
// 判断是否有足够的距离进行减速
if (decdis >= fDis)
{
// 如果减速所需距离已经超过或等于总距离，调整最大速度
// 使用公式 v^2 = u^2 + 2as 解出 v
vel = (float)Math.Sqrt(2 * facc * fDis);
// 重新计算减速时间
dectime = vel / facc;
}
// 计算匀速阶段时间
float unftime = 0.0f;
if (decdis < fDis)
{
// 如果有剩余距离进行匀速运动
unftime = (fDis - decdis) / vel;
}
// 总飞行时间 = 匀速阶段时间 + 减速阶段时间
return unftime + dectime;
}
//单算减速，不算加速的最大飞行速度
public float CalculateMaximumVelocity(float distance, float time, float acceleration)
{
// 二次方程系数
float a_coeff = 1;
float b_coeff = -2 * time;
float c_coeff = (2 * distance) / acceleration;
// 计算判别式
float discriminant = b_coeff * b_coeff - 4 * a_coeff * c_coeff;
if (discriminant < 0)
{
return -1; // 无实数解
}
// 计算二次方程的根
float t1_root1 = (-b_coeff + (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
float t1_root2 = (-b_coeff - (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
// 选择合理的根作为 t1
float t1 = Math.Min(t1_root1, t1_root2);
if (t1 < 0 || t1 > time)
{
return -1; // 无合理解
}
// 计算最大速度 v
return acceleration * t1;
}
//单算加速，不算减速的距离速度计算
float getspeeddist_V2(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
{
vdist = 0;
vspeed = 0;
float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
{
vspeed = Distance / flight_time;
vdist = vspeed * curr_time; //匀速距离
return 0;
}
float dect = desV / fc_acc; // 总加速时间
float unit = flight_time - dect; //匀速段时间
float decD = (fc_acc * dect * dect) / 2; //总加速距离
if (dect > flight_time) dect = flight_time;//约束时间
if (curr_time > dect) //大于加速段时间--匀速
{
vspeed = (Distance - decD) / (flight_time - dect);
vdist = vspeed * (curr_time - dect);
vdist = vdist + decD; //总距离=匀速距离+减速距离
if (vdist > Distance) vdist = Distance; //约束距离
}
else //加速段
{
vspeed = fc_acc * curr_time;
vdist = (fc_acc * curr_time * curr_time) / 2; //加速距离
}
return 0;
}
//单算减速，不算加速的距离速度计算
float getspeeddist_V3(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
{
vdist = 0;
vspeed = 0;
float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
{
vspeed = Distance / flight_time;
vdist = vspeed * curr_time; //匀速距离
return 0;
}
float dect = desV / fc_acc; // 总减速时间
float unit = flight_time - dect; //匀速段时间
float decD = desV * dect - (fc_acc * dect * dect) / 2; //总减速距离
if (dect > flight_time) dect = flight_time;//约束时间
//匀速时间内
if (curr_time < unit)
{
vspeed = (Distance - decD) / (flight_time - dect);
vdist = vspeed * curr_time;
}
else
{
if (((flight_time - dect) * unit) == 0)
vdist = 0;
else
vdist = (Distance - decD) / (flight_time - dect) * unit; //匀速距离
float currdect = curr_time - unit; //减速时间
if (currdect >= 0)
{
vspeed = desV - fc_acc * currdect;
decD = desV * currdect - (fc_acc * currdect * currdect) / 2; //减速距离
}
vdist = vdist + decD; //总距离=匀速距离+减速距离
if (vdist > Distance) vdist = Distance; //约束距离
}
return 0;
}
float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed)
{
float fDis = fabsf(Distance);
float facc = fabsf(fc_acc);
float realflytime = 0.0f;
//计算一半的距离
float hafdis = (fDis / 2);
//计算最大速度
float vel = (float)sqrt(2 * facc * hafdis);
//如果速度超过最大速度
if (vel > fc_maxspeed)
//使用最大速度
vel = fc_maxspeed;
//加速时间
float acctim = vel / facc;
//加速距离
float accdis = vel * vel / (2 * facc);
//匀速段时间
float vtime = (hafdis - accdis) / vel;
//到一半的时间
float haftime = acctim + vtime;
realflytime = haftime * 2;
return realflytime;
}
///计算飞行距离和速度 单位--米，秒----
//返回 整个距离最大飞行速度
///flight_time 总飞行时间 Distance 飞行总距离 curr_time 当前飞行时间 fc_acc加速度, fc_maxspeed最大速度 vdist 计算的应该飞的距离 vspeed 计算的当前时间应该速度
//========这是飞控正在使用的算法========
float getspeeddist_V1(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
{
float plan_flytime=_plan_flytime;
float desired_velocity=_desired_velocity;
float dist = 0;
//导航加速度 米/秒*秒 ---不使用导航库
float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
//
float speed = 0;
//计算实时速度
if (curr_time <= (plan_flytime / 2.0))
speed = curr_time * wpacc;
else
//需要减速
speed = (plan_flytime - curr_time) * wpacc;
//不能大于目标速度
if (speed > desired_velocity)
speed = desired_velocity;
if (speed <= 0)
speed = 0;
vspeed = speed;
//计算实时距离
float V_start = 0.0f;
float V_end = 0.0f;
//加速段
float t1 = (desired_velocity - V_start) / wpacc;
//减速段
float t3 = (desired_velocity - V_end) / wpacc;
//平飞段
float t2 = plan_flytime - t1 - t3;
if (curr_time < t1)
{
dist = 0.5f * wpacc * curr_time * curr_time + curr_time * V_start;
}
else if (curr_time < t1 + t2)
{
dist = -0.5f * wpacc * t1 * t1 + (curr_time) * desired_velocity;
}
else
{
float t = curr_time - t1 - t2;
dist = -0.5f * wpacc * t1 * t1 + curr_time * desired_velocity - 0.5f * wpacc * t * t;
}
if (fabsf(dist) > fabsf(Distance))
vdist = Distance;
else
{
if (Distance < 0)
vdist = -dist;
else vdist = dist;
}
return desired_velocity;
}
float _vspeed=0;
float _desired_velocity = 0;
float _plan_flytime = 0;
float initgetspeeddist_V1(float flight_time, float Distance, float fc_acc, float fc_maxspeed)
{
//计划飞行时间
float plan_flytime = flight_time;
//计算距离用绝对值
float fDis = fabsf(Distance);
//导航加速度 米/秒*秒 ---不使用导航库
float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
//最大目标速度---米/秒
float desired_velocity = 0;
//计算最小飞行时间
float minflytime = getMinfligthtime(Distance, fc_acc, fc_maxspeed);
if (flight_time < minflytime)
plan_flytime = minflytime;// + 0.1f;
//根据总飞行时间和总距离计算飞行速度----起始和结束速度都是0，中间按匀加速和匀减速计算，没考虑加加速度
float delta = (0.5f * plan_flytime) * (0.5f * plan_flytime) - fDis / wpacc;
if (delta >= 0)
{
desired_velocity = (0.5f * plan_flytime - sqrt(delta)) / (1 / wpacc);
if (desired_velocity > fc_maxspeed)
{
plan_flytime = minflytime;
desired_velocity = fc_maxspeed;
}
}
else
{
desired_velocity = (0.5f * plan_flytime) / (1 / wpacc);
}
if (desired_velocity < 0.2f) desired_velocity = 0.2f; //限制最小速度0.2米/秒
if (desired_velocity > fc_maxspeed) desired_velocity = fc_maxspeed;//限制最大速度10米/秒，应该参数化
_desired_velocity = desired_velocity;
_plan_flytime = plan_flytime;
return desired_velocity;
}
public float initgetspeeddist(int flytype, float flight_time, float Distance, float fc_acc, float fc_maxspeed)
{
flytype = 0;
switch (flytype)
{
case 0: //匀速
_vspeed = Distance / flight_time;
return 0;
case 1: //同时计算加减速
return initgetspeeddist_V1(flight_time, Distance, fc_acc, fc_maxspeed);
case 2: //单加速
return 0;
case 3: //单减速
return 0;
default:
return 0;
}
}
public float getspeeddist(int flytype, float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
{
flytype = 0;
switch (flytype)
{
case 0: //匀速
//case 1: //匀速
vspeed = _vspeed;
vdist = vspeed * curr_time;
return 0;
case 1: //同时计算加减速
return getspeeddist_V1(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
case 2: //单加速
return getspeeddist_V2(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
case 3: //单减速
return getspeeddist_V3(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
default:
vspeed = 0;
vdist = 0;
return 0;
}
}
}
}

2
PlaneGcsSdk_Shared/CopterControllers/KeyboardController.cs
View File

@ -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);
} }

2
PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs
View File

@ -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) public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
{ {
return Copter.FlyToAsync(lat, lng, alt); return Copter.FlyToAsync(lat, lng, alt);
} }

2830
PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
View File

File diff suppressed because it is too large Load Diff

626
PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs
View File

@ -1,309 +1,317 @@
using System; using System;
using static Plane.Protocols.MAVLink; using System.Collections.Generic;
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) {
{
IsConnected = _internalCopter.IsConnected; private bool _fetchingFirmwareVersion;
}
private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
private void _internalCopter_GetLogDataEvent(string log) {
{ IsConnected = _internalCopter.IsConnected;
//飞机消息日志，后面需要改为记录方式 }
StatusText =Name+":"+log;
var e = new MessageCreatedEventArgs { Message = StatusText }; private void _internalCopter_GetLogDataEvent(string log)
RaiseTextReceived(e); {
} //飞机消息日志，后面需要改为记录方式
StatusText =Name+":"+log;
private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType) var e = new MessageCreatedEventArgs { Message = StatusText };
{ RaiseTextReceived(e);
switch (StreamType) }
{
case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
{ private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
SatCount = _internalCopter.satcount; {
GpsFixType = _internalCopter.gpsstatus.ToGpsFixType(); switch (StreamType)
GpsHdop = _internalCopter.gpshdop; {
Altitude = _internalCopter.gpsalt; case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
Retain = BitConverter.GetBytes(_internalCopter.retain); {
SatCount = _internalCopter.satcount;
Voltage = _internalCopter.battery_voltage; GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
CommModuleMode = (FlightMode)_internalCopter.commModuleMode; GpsHdop = _internalCopter.gpshdop;
CommModuleVersion = _internalCopter.commModuleVersion; CopterErrorCode =_internalCopter.errorcode;
IsUnlocked = _internalCopter.isUnlocked; CopterPreCheckPass = _internalCopter.precheckok;
//Yaw = _internalCopter.yaw; CopterErrorString = getcoptererrorstr(_internalCopter.errorcode);
Heading = _internalCopter.heading; Altitude = _internalCopter.gpsalt;
HeartbeatCount++; Retain = BitConverter.GetBytes(_internalCopter.retain);
if (SatCount > 8) Voltage = _internalCopter.battery_voltage;
{ CommModuleMode = (FlightMode)_internalCopter.commModuleMode;
IsGpsAccurate = true; CommModuleVersion = _internalCopter.commModuleVersion;
RecordLat = _internalCopter.lat; IsUnlocked = _internalCopter.isUnlocked;
RecordLng = _internalCopter.lng; //Yaw = _internalCopter.yaw;
} Heading = _internalCopter.heading;
HeartbeatCount++;
Latitude = RecordLat;
Longitude = RecordLng; if (SatCount > 8)
UpdateFlightDataIfNeeded(); {
RaiseLocationChangedIfNeeded(); IsGpsAccurate = true;
RecordLat = _internalCopter.lat;
RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT); RecordLng = _internalCopter.lng;
}
break;
} Latitude = RecordLat;
case MAVLINK_MSG_ID_ATTITUDE: //无用 Longitude = RecordLng;
{ UpdateFlightDataIfNeeded();
Roll = _internalCopter.roll; RaiseLocationChangedIfNeeded();
Pitch = _internalCopter.pitch;
Yaw = _internalCopter.yaw; RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
TimebootMs = _internalCopter.timebootms;
RaiseAttitudeChanged(); break;
}
break; case MAVLINK_MSG_ID_ATTITUDE: //无用
} {
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要 Roll = _internalCopter.roll;
{ Pitch = _internalCopter.pitch;
Channel1 = _internalCopter.ch1in; Yaw = _internalCopter.yaw;
Channel2 = _internalCopter.ch2in; TimebootMs = _internalCopter.timebootms;
Channel3 = _internalCopter.ch3in; RaiseAttitudeChanged();
Channel4 = _internalCopter.ch4in;
Channel5 = _internalCopter.ch5in; break;
Channel6 = _internalCopter.ch6in; }
Channel7 = _internalCopter.ch7in; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要
Channel8 = _internalCopter.ch8in; {
Channel1 = _internalCopter.ch1in;
RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW); Channel2 = _internalCopter.ch2in;
Channel3 = _internalCopter.ch3in;
break; Channel4 = _internalCopter.ch4in;
} Channel5 = _internalCopter.ch5in;
case MAVLINK_MSG_ID_VFR_HUD: Channel6 = _internalCopter.ch6in;
{ Channel7 = _internalCopter.ch7in;
Heading = _internalCopter.heading; //有用 Channel8 = _internalCopter.ch8in;
Altitude = _internalCopter.alt; //有用
AirSpeed = _internalCopter.airspeed; //没用 RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);
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;
Mode = (FlightMode)_internalCopter.mode; break;
// 林俊清, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。 }
HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0; }
++HeartbeatCount; }
IsCheckingConnection = false; private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
/* {
if (FirmwareVersion == null && !_fetchingFirmwareVersion) IsUnlocked = _internalCopter.armed;
{ Mode = (FlightMode)_internalCopter.mode;
try // 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
{ HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
_fetchingFirmwareVersion = true; ++HeartbeatCount;
FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
} IsCheckingConnection = false;
catch (TimeoutException) /*
{ if (FirmwareVersion == null && !_fetchingFirmwareVersion)
// 吞掉。 {
} try
finally {
{ _fetchingFirmwareVersion = true;
_fetchingFirmwareVersion = false; FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
} }
} catch (TimeoutException)
*/ {
RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount)); // 吞掉。
} }
finally
private void _internalCopter_ReceiveSensorEvent(object sender) {
{ _fetchingFirmwareVersion = false;
RaiseSensorDataReceived(EventArgs.Empty); }
} }
*/
private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff) RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
{ }
Voltage = _internalCopter.battery_voltage / 1000;
private void _internalCopter_ReceiveSensorEvent(object sender)
if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。 {
{ RaiseSensorDataReceived(EventArgs.Empty);
CalcBatteryPer(); }
}
else private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff)
{ {
BatteryPer = _internalCopter.battery_remaining; Voltage = _internalCopter.battery_voltage / 1000;
}
if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。
const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308; {
const int MIN_VERSION_OF_GENERATION2 = 0x3000; CalcBatteryPer();
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;
else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2) const int MIN_VERSION_OF_GENERATION2 = 0x3000;
{ if (FirmwareVersion != null)
// 2.0 飞行器，飞控未提供 canTakeOff。 {
IsGpsAccurate = SatCount >= 12; if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
} {
else // 飞控提供了 canTakeOff。
{ IsGpsAccurate = canTakeOff;
// 1.0 飞行器，飞控不提供 canTakeOff。 }
IsGpsAccurate = SatCount >= 6; else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
} {
} // 2.0 飞行器，飞控未提供 canTakeOff。
else IsGpsAccurate = SatCount >= 12;
IsGpsAccurate = SatCount >= 8; }
else
FlightDistance2D = _internalCopter.FlightDistance2D; {
// 1.0 飞行器，飞控不提供 canTakeOff。
RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth)); IsGpsAccurate = SatCount >= 6;
} }
}
private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e) else
{ IsGpsAccurate = SatCount >= 8;
switch (e.Packet[5])
{ FlightDistance2D = _internalCopter.FlightDistance2D;
case MAVLINK_MSG_ID_MISSION_COUNT:
MissionCount = _internalCopter.WpCount; RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
break; }
default: private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
break; {
} switch (e.Packet[5])
} {
case MAVLINK_MSG_ID_MISSION_COUNT:
private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e) MissionCount = _internalCopter.WpCount;
{ break;
switch (e.Packet[5])
{ default:
case MAVLINK_MSG_ID_MISSION_ACK: break;
AnalyzeMissionAckPacket(e.Packet); }
break; }
case MAVLINK_MSG_ID_MISSION_COUNT: private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
AnalyzeMissionCountPacket(e.Packet); {
break; switch (e.Packet[5])
{
case MAVLINK_MSG_ID_MISSION_ITEM: case MAVLINK_MSG_ID_MISSION_ACK:
AnalyzeMissionItemPacket(e.Packet); AnalyzeMissionAckPacket(e.Packet);
break; break;
case MAVLINK_MSG_ID_MISSION_REQUEST: case MAVLINK_MSG_ID_MISSION_COUNT:
AnalyzeMissionRequestPacket(e.Packet); AnalyzeMissionCountPacket(e.Packet);
break; break;
case MAVLINK_MSG_ID_SET_PAIR: case MAVLINK_MSG_ID_MISSION_ITEM:
AnalyzeSetPairPacket(e.Packet); AnalyzeMissionItemPacket(e.Packet);
break; break;
default: case MAVLINK_MSG_ID_MISSION_REQUEST:
break; AnalyzeMissionRequestPacket(e.Packet);
} break;
}
case MAVLINK_MSG_ID_SET_PAIR:
#if PRIVATE AnalyzeSetPairPacket(e.Packet);
protected virtual void RegisterInternalCopterEventHandlers() break;
#else
default:
private void RegisterInternalCopterEventHandlers() break;
#endif }
{ }
_internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
_internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent; #if PRIVATE
_internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent; protected virtual void RegisterInternalCopterEventHandlers()
_internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent; #else
_internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
_internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken; private void RegisterInternalCopterEventHandlers()
_internalCopter.PacketHandled += _internalCopter_PacketHandled; #endif
_internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived; {
_internalCopter.MessageCreated += _internalCopter_MessageCreated; _internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
} _internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
_internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e) _internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
{ _internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message }; _internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
RaiseTextReceived(txte); _internalCopter.PacketHandled += _internalCopter_PacketHandled;
} _internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
_internalCopter.MessageCreated += _internalCopter_MessageCreated;
#region 计算剩余电量 }
private int bPerTimes; private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e)
private int outBatteryPer; {
private int[] tPerTimes = new int[20]; var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message };
private int v_num; RaiseTextReceived(txte);
}
/// <summary>
/// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。 #region 计算剩余电量
/// </summary>
private void CalcBatteryPer() private int bPerTimes;
{ private int outBatteryPer;
float volmax = 0f; private int[] tPerTimes = new int[20];
float volmin = 0f; private int v_num;
if (Voltage > 5 && Voltage < 9)
{ /// <summary>
volmax = 8.2f; /// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
volmin = 7f; /// </summary>
} private void CalcBatteryPer()
else if (Voltage >= 9 {
&& Voltage < 13.6) float volmax = 0f;
{ float volmin = 0f;
volmax = 11.6f; if (Voltage > 5 && Voltage < 9)
volmin = 10.2f; {
} volmax = 8.2f;
else if (Voltage >= 13.6 volmin = 7f;
&& Voltage < 17.2) }
{ else if (Voltage >= 9
volmax = 16.3f; && Voltage < 13.6)
volmin = 14.2f; {
} volmax = 11.6f;
else if (Voltage >= 17.2 volmin = 10.2f;
&& Voltage < 26.2) }
{ else if (Voltage >= 13.6
volmax = 24.8f; && Voltage < 17.2)
volmin = 21.2f; {
} volmax = 16.3f;
volmin = 14.2f;
int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin)); }
else if (Voltage >= 17.2
if (batteryPer == -1 || volmax == 0 || volmin == 0) && Voltage < 26.2)
return; {
volmax = 24.8f;
if (bPerTimes < 20) volmin = 21.2f;
{ }
tPerTimes[bPerTimes] = batteryPer;
bPerTimes += 1; int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));
}
else if (batteryPer == -1 || volmax == 0 || volmin == 0)
{ return;
tPerTimes[v_num] = batteryPer;
v_num++; if (bPerTimes < 20)
if (v_num == 20) {
v_num = 0; tPerTimes[bPerTimes] = batteryPer;
} bPerTimes += 1;
for (int i = 0; i < bPerTimes; i++) }
{ else
outBatteryPer += tPerTimes[i]; {
} tPerTimes[v_num] = batteryPer;
v_num++;
outBatteryPer = outBatteryPer / bPerTimes; if (v_num == 20)
if (outBatteryPer < BatteryPer && bPerTimes > 18) v_num = 0;
{ }
BatteryPer = (byte)outBatteryPer; for (int i = 0; i < bPerTimes; i++)
} {
} outBatteryPer += tPerTimes[i];
}
#endregion 计算剩余电量
} outBatteryPer = outBatteryPer / bPerTimes;
} if (outBatteryPer < BatteryPer && bPerTimes > 18)
{
BatteryPer = (byte)outBatteryPer;
}
}
#endregion 计算剩余电量
}
}

2
PlaneGcsSdk_Shared/Copters/EmptyCopter.cs
View File

@ -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) protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
{ {
return TaskUtils.CompletedTask; return TaskUtils.CompletedTask;
} }

1790
PlaneGcsSdk_Shared/Copters/FakeCopter.cs
View File

File diff suppressed because it is too large Load Diff

170
PlaneGcsSdk_Shared/Copters/FlightMode.cs
View File

@ -1,84 +1,86 @@
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 }
{
public static string GetModeString(this FlightMode flightMode) internal static class FightModeExtensions
{ {
switch (flightMode) public static string GetModeString(this FlightMode flightMode)
{ {
case FlightMode.ALT_HOLD: switch (flightMode)
return "ALT HOLD"; {
case FlightMode.ALT_HOLD:
case FlightMode.POSITION: return "ALT HOLD";
return "POS HOLD";
case FlightMode.POSITION:
default: return "POS HOLD";
return Enum.GetName(typeof(FlightMode), flightMode);
} default:
} return Enum.GetName(typeof(FlightMode), flightMode);
}
public static bool NeedGps(this FlightMode flightMode) }
{
switch (flightMode) public static bool NeedGps(this FlightMode flightMode)
{ {
case FlightMode.AUTO: switch (flightMode)
case FlightMode.GUIDED: {
case FlightMode.LOITER: case FlightMode.AUTO:
case FlightMode.RTL: case FlightMode.GUIDED:
case FlightMode.CIRCLE: case FlightMode.LOITER:
case FlightMode.LAND: case FlightMode.RTL:
case FlightMode.POSITION: case FlightMode.CIRCLE:
default: case FlightMode.LAND:
return true; case FlightMode.POSITION:
default:
case FlightMode.STABILIZE: return true;
case FlightMode.ACRO:
case FlightMode.ALT_HOLD: case FlightMode.STABILIZE:
case FlightMode.OF_LOITER: case FlightMode.ACRO:
case FlightMode.TOY: case FlightMode.ALT_HOLD:
return false; case FlightMode.OF_LOITER:
} case FlightMode.TOY:
} return false;
}
internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode) }
{
return (PlaneCopter.ac2modes)flightMode; internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode)
} {
} return (PlaneCopter.ac2modes)flightMode;
} }
}
}

6
PlaneGcsSdk_Shared/Copters/PLCopter.cs
View File

@ -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.ALT_HOLD).ConfigureAwait(false)) if (await SetModeAsync(FlightMode.LOITER).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) protected override async Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
{ {
if (!IsEmergencyHoverActive) if (!IsEmergencyHoverActive)
{ {

2
PlaneGcsSdk_Shared/Copters/PlaneCopter.NestedTypes.cs
View File

@ -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);

806
PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs
View File

@ -1,402 +1,404 @@
//#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;
gpsalt = ((float)info.alt) / 1000; errorcode= (byte)info.error_code;
satcount = info.gps_num_sats; precheckok = info.rpecheck_fault == 0;
isUnlocked = info.lock_status == 1; gpsalt = ((float)info.alt) / 1000;
satcount = info.gps_num_sats;
battery_voltage = ((float)info.battery_voltage) / 1000; isUnlocked = info.lock_status == 1;
heading = (short)((info.heading / 100) % 360); battery_voltage = ((float)info.battery_voltage) / 1000;
commModuleVersion = version; heading = (short)((info.heading / 100) % 360);
retain = info.retain; commModuleVersion = version;
RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT);
} retain = info.retain;
} RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT);
} }
}
}

3356
PlaneGcsSdk_Shared/Copters/PlaneCopter.cs
View File

File diff suppressed because it is too large Load Diff

1
PlaneGcsSdk_Shared/PlaneGcsSdk_Shared.projitems
View File

@ -34,6 +34,7 @@
<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" />

12
PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs
View File

@ -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 //协议头 飞控4.0以后 254 - 240 Mavlink头 FE 改为 F0
//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, 来自郭老师，据说是传文件用的。 /// // 王海, 20160331, 来自王海1，据说是传文件用的。
/// </summary> /// </summary>
TELL_COPTER = 23, TELL_COPTER = 23,
/// <summary> /// <summary>
/// // 林俊清, 20160331, 来自郭老师的邮件，作用未知。 /// // 王海, 20160331, 来自王海1的邮件，作用未知。
/// </summary> /// </summary>
FILE_TRANS_MODE = 24, FILE_TRANS_MODE = 24,
/// <summary> /// <summary>
/// // 林俊清, 20160331, 来自郭老师的邮件，用于 RF 测试。 /// // 王海, 20160331, 来自王海1的邮件，用于 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;

347
PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs
View File

@ -1,160 +1,187 @@
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> /// 发送非针对飞控的内部控制指令
public const short COMM_NOTIFICATION = 0x1234; /// </summary>
public const short COMM_TO_MODULE = 0x5F;
/// <summary>
/// 空中升级(更新通信模块飞机端)
/// </summary>
public const short COMM_UPDATE_COPTER_MODULE = 0xFD; /// <summary>
/// 通信模块
#endregion /// </summary>
public const short COMM_NOTIFICATION = 0x1234;
public enum CommMode
{ /// <summary>
IDLE = 0, /// 空中升级(更新通信模块飞机端)
SEARCH = 1, /// </summary>
DOWNLOAD = 3, public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
FLIGHT = 4
}
public enum MessageType /// <summary>
{ /// 测试模块
STR = 0, /// </summary>
SCAN2 = 2, public const short COMM_TEST_MODULE = 0x3C;
SCAN3 = 3
}
//search 搜索模式相关 #endregion
public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功
public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功 public enum CommMode
public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标 {
public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误 IDLE = 0,
public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S SEARCH = 1,
DOWNLOAD = 3,
FLIGHT = 4
public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息, }
//需要再后续等待一个成功消息，才算完成
public enum MessageType
public const ushort P2P_SEND_FAILED = 0x0600; //点对点通信发送失败 {
STR = 0,
public const ushort ERROR_CODE_START = 0x0100; SCAN2 = 2,
public const ushort ERROR_CODE_END = 0x03FF; SCAN3 = 3
}
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] //search 搜索模式相关
public struct comm_set_mav_count public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功
{ public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功
public Int16 mav_count; //飞机总数 public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标
}; 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;
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)] public const ushort ERROR_CODE_END = 0x03FF;
public struct comm_asyn_time
{
public Int64 time_stamp; [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
}; public struct comm_set_mav_count
{
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)] public Int16 mav_count; //飞机总数
public struct comm_copter_info };
{
public Int32 control_mode; [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
public UInt32 lat; public struct comm_update_copter_module
public UInt32 lng; {
public float retain; public Int16 mav_count; //飞机总数
public Int32 alt; public Int16 update_code;
};
public Int16 gps_status;
public byte lock_status; [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
public byte gps_num_sats; public struct comm_asyn_time
public Int16 battery_voltage; {
public Int16 heading; public Int64 time_stamp;
};
};
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
} public struct comm_copter_info
} {
// public Int32 control_mode; //飞行模式(自动，悬停，定高，返航)
public byte control_mode; //飞行模式(自动，悬停，定高，返航)
public byte rpecheck_fault; //是否有 预解锁错误
public byte reserveddata1; //保留
public byte reserveddata2; //保留
public UInt32 lat; // 经度 in 1E7 degrees
public UInt32 lng; // 维度 in 1E7 degrees
public float retain; // 写参数序列号,返回版本号等不特定返回数据
public Int32 alt; // millimeters above home
public byte gps_status; //锁定类型 (无锁定,3D锁定，RTK浮点，RTK固定)
public byte error_code; //错误号，0表示无错误 --放到低位为了和之前兼容
public byte lock_status; //以及是否解锁
public byte gps_num_sats; // 卫星数量
public Int16 battery_voltage; // 电池电压mV
public UInt16 heading; //方向角度
};
}
}

2
PlaneGcsSdk_Shared/Protocols/MavlinkUtil.cs
View File

@ -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);

2
PlaneGcsSdk_Shared/Protocols/MavlinkUtil_NET45.cs
View File

@ -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);

4
PlaneGcsSdk_Shared/TaskTools/HIL/Vector3.cs
View File

@ -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)
//{ //{