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

1增加观测者位置图标，修改后直接可以使用

Browse Source 
2增加配置FC_VER_NO = 3 使用小航点同时达到模式计算模拟飞行
3增加灯光框架，可实现闪烁等，但实际使用太慢，暂时不用
This commit is contained in:
pxzleo 2023-11-21 21:18:03 +08:00
parent fc9b2595d6
commit d051300171
10 changed files with 313 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -34,15 +34,17 @@
<WarningLevel>4</WarningLevel>
<DocumentationFile>bin\Release\PlaneGcsSdk.Contract_Private.xml</DocumentationFile>
</PropertyGroup>
<ItemGroup>
<!-- A reference to the entire .NET Framework is automatically included -->
</ItemGroup>
<ItemGroup>
<Compile Include="Copters\DataStreamReceivedEventArgs.cs" />
<Compile Include="Copters\DataStreamType.cs" />
<Compile Include="Copters\ICopterActions.cs" />
<Compile Include="Properties\AssemblyInfo.cs" />
</ItemGroup>
<ItemGroup>
<Reference Include="System.Drawing">
<HintPath>C:\Program Files (x86)\Reference Assemblies\Microsoft\Framework\.NETFramework\v4.0\System.Drawing.dll</HintPath>
</Reference>
</ItemGroup>
<Import Project="..\PlaneGcsSdk.Contract_Shared\PlaneGcsSdk.Contract_Shared.projitems" Label="Shared" />
<Import Project="$(MSBuildExtensionsPath32)\Microsoft\Portable\$(TargetFrameworkVersion)\Microsoft.Portable.CSharp.targets" />
<!-- To modify your build process, add your task inside one of the targets below and uncomment it.

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

@ -31,7 +31,8 @@ namespace Plane.Copters
/// <param name="lng">经度。</param>
/// <param name="alt">相对于解锁点的高度。</param>
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
Task FlyToAsync(double lat, double lng, float alt);
/// flytime =飞行时间 秒
Task FlyToAsync(double lat, double lng, float alt,float flytime=0);
/// <summary>
/// 切换到 <see cref="CopterState.HoverMode"/> 并悬停。此操作需要使用 GPS 定位，卫星数不足导致定位不准时非常危险。

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

@ -1,5 +1,6 @@
using Plane.Geography;
using System;
using System.Drawing;
namespace Plane.Copters
{
@ -187,5 +188,19 @@ namespace Plane.Copters
/// LED颜色
/// </summary>
string LEDColor { get; set; }
/// <summary>
/// LED灯光模式 0常亮 1闪烁 2随机闪烁(RGB无意义)
/// </summary>
int LEDMode { get; set; }
/// <summary>
/// LED变化间隔
/// </summary>
float LEDInterval { get; set; }
/// <summary>
/// LED显示颜色---内部计算后的真实颜色，用于显示
/// </summary>
Color LEDShowColor { get; set; }
}
}

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);
}
/// <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>
/// 计算空间中两点之间的距离，单位为米。

1
PlaneGcsSdk_Private_NET46/PlaneGcsSdk_Private_NET46.csproj
View File

@ -34,6 +34,7 @@
<Reference Include="PresentationCore" />
<Reference Include="System" />
<Reference Include="System.Core" />
<Reference Include="System.Drawing" />
<Reference Include="System.Xml.Linq" />
<Reference Include="System.Data.DataSetExtensions" />
<Reference Include="Microsoft.CSharp" />

2
PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs
View File

@ -104,7 +104,7 @@ namespace Plane.CopterManagement
return Copter.FlyToAsync(lat, lng);
}
public Task FlyToAsync(double lat, double lng, float alt)
public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0)
{
return Copter.FlyToAsync(lat, lng, alt);
}

121
PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
View File

@ -1,6 +1,7 @@
using Plane.Geography;
using System;
using System.ComponentModel;
using System.Drawing;
using System.Threading;
using System.Threading.Tasks;
@ -104,6 +105,7 @@ namespace Plane.Copters
private double _FlightDistance;
private double _FlightDistance2D;
private int _FlightControlMode;
private TimeSpan _FlightTime;
@ -480,6 +482,15 @@ namespace Plane.Copters
protected set { Set(nameof(FlightDistance2D), ref _FlightDistance2D, value); }
}
//FlightControlMode=1表示同时到达，速度可变,用于新固件
//=0表示固定速度，用于老固件
public int FlightControlMode
{
get { return _FlightControlMode; }
set { Set(nameof(FlightControlMode), ref _FlightControlMode, value); }
}
public TimeSpan FlightTimeSpan
{
get { return _FlightTime; }
@ -737,12 +748,59 @@ namespace Plane.Copters
{
get { return _LEDColor; }
set {
if (Set(nameof(LEDColor), ref _LEDColor, value))
if (value!=_LEDColor)
{
Set(nameof(LEDColor), ref _LEDColor, value);
//强制刷新颜色--在刷新位置时才刷新颜色
RefreashLoc();
RefreashLoc();
}
}
}
private Color _LEDShowColor;
public Color LEDShowColor
{
get { return _LEDShowColor; }
set
{
Set(nameof(LEDShowColor), ref _LEDShowColor, value);
}
}
private int _LEDMode;
public int LEDMode
{
get { return _LEDMode; }
set
{
if (value != _LEDMode)
{
Set(nameof(LEDMode), ref _LEDMode, value);
//强制刷新颜色--在刷新位置时才刷新颜色
// RefreashLoc();
}
}
}
private float _LEDInterval;
public float LEDInterval
{
get { return _LEDInterval; }
set
{
if (value != _LEDInterval)
{
Set(nameof(LEDInterval), ref _LEDInterval, value);
//强制刷新颜色--在刷新位置时才刷新颜色
// RefreashLoc();
}
}
}
#if PRIVATE
public
#else
@ -807,67 +865,16 @@ namespace Plane.Copters
return FlyToAsync(lat, lng, Altitude);
}
public Task FlyToAsync(double lat, double lng, float alt)
public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0)
{
_shouldFollow = false;
//State = CopterState.CommandMode;
return FlyToCoreAsync(lat, lng, alt);
return FlyToCoreAsync(lat, lng, alt, flytime);
}
public void Follow(IVisibleStatus target, bool keepYawDifference = false, bool keepFacingTarget = true, bool keep3DRelativeLocations = false)
{
// 王海, 20160409, 在目前的实现中，跟随状态使用 GUIDED 模式。
if (!IsConnected || !IsUnlocked || this == target || IsEmergencyHoverActive) return;
_followTarget = target;
_followAltDifference = Altitude - target.Altitude;
_followDistance = (float)target.CalcDistance2D(this);
_followKeepYawDifference = keepYawDifference;
_followKeepFacingTarget = keepFacingTarget;
_followKeep3DRelativeLocations = keep3DRelativeLocations;
_followSelfDirectionFromTarget = (float)target.CalcDirection2D(this).RadToDeg();
_followTargetOriginalYaw = target.Yaw.NormalizeDirection();
if (State == CopterState.Following) return;
Task.Run(async () =>
{
State = CopterState.Following;
_shouldFollow = true;
while (IsConnected && _shouldFollow)
{
// 计算飞行器应当处于什么位置，并调用 FlyTo 使其飞往彼处。
var destination2D = _followTarget.CalcLatLngSomeMetersAway2D(_followKeepYawDifference ? (_followSelfDirectionFromTarget - _followTargetOriginalYaw + _followTarget.Yaw.NormalizeDirection()).NormalizeDirection() : _followSelfDirectionFromTarget, _followDistance);
var destinationAlt = _followKeep3DRelativeLocations ? _followTarget.Altitude + _followAltDifference : Altitude;
var destination = new PLLocation(destination2D.Latitude, destination2D.Longitude, destinationAlt);
if (_followLastDestination == null || _followLastDestination.CalcDistance(destination) >= 1.5)
{
await FlyToCoreAsync(destination2D.Latitude, destination2D.Longitude, destinationAlt).ConfigureAwait(false);
_followLastDestination = destination;
}
// 如果需要保持面对目标，计算并使用偏航和云台俯仰的期望值。
if (_followKeepFacingTarget)
{
var yaw = (float)this.CalcDirection2D(_followTarget).RadToDeg();
SetFieldAndRaisePropertyChanged(ref _DesiredYaw, yaw, nameof(DesiredYaw));
var distance2DFromTargetToCopter = _followTarget.CalcDistance2D(this);
var altDifferenceFromTargetToCopter = Altitude - _followTarget.Altitude;
// 正前方值为 0，向下取正值，向上取负值。
var gimbalPitchRad = Math.Atan2(altDifferenceFromTargetToCopter, distance2DFromTargetToCopter);
// 正前方值为 1500，向下 80° 值为 1900。
var ch7 = (ushort)((1900 - 1500) * gimbalPitchRad / 80F.DegToRad() + 1500);
SetFieldAndRaisePropertyChanged(ref _DesiredChannel7, ch7, nameof(DesiredChannel7));
}
await Task.Delay(50).ConfigureAwait(false);
}
if (!IsConnected) _shouldFollow = false;
});
}
public async Task HoverAsync()
@ -1118,7 +1125,7 @@ namespace Plane.Copters
/// <param name="lng">经度。</param>
/// <param name="alt">高度。</param>
/// <returns>表示此异步发送操作的 <see cref="Task{TResult}"/> 实例。</returns>
protected abstract Task FlyToCoreAsync(double lat, double lng, float alt);
protected abstract Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0);
protected void RaiseAltitudeChanged() => AltitudeChanged?.Invoke(this, EventArgs.Empty);

2
PlaneGcsSdk_Shared/Copters/EmptyCopter.cs
View File

@ -144,7 +144,7 @@ namespace Plane.Copters
return Task.FromResult(true);
}
protected override Task FlyToCoreAsync(double lat, double lng, float alt)
protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0)
{
return TaskUtils.CompletedTask;
}

198
PlaneGcsSdk_Shared/Copters/FakeCopter.cs
View File

@ -2,10 +2,11 @@
using Plane.Geography;
using System;
using System.Diagnostics;
using System.Drawing;
using System.Threading;
using System.Threading.Tasks;
using static Plane.Copters.Constants;
using System.Windows.Media;
namespace Plane.Copters
{
/// <summary>
@ -21,17 +22,18 @@ namespace Plane.Copters
/// <summary>
/// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
/// </summary>
static private int UPDATE_INTERVAL =50; //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架
static private int UPDATE_INTERVAL =100; //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架
static private int UPDATE_INTERVAL_TEMP = 50;
/// <summary>
/// 在一个更新间隔中的最大移动距离。
/// </summary>
private float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL / 1000;
private float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000;
/// <summary>
/// 高速模式下，在一个更新间隔中的最大移动距离。
/// </summary>
private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL / 1000 * 4; //MAX_MOVE_IN_INTERVAL*4
private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4; //MAX_MOVE_IN_INTERVAL*4
@ -55,12 +57,12 @@ namespace Plane.Copters
/// <summary>
/// 高速模式下，按比例缩放过的在一个更新间隔中的最大移动距离。
/// </summary>
private float _scaledFastMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL / 1000 * 4; //MAX_MOVE_IN_INTERVAL_FAST
private float _scaledFastMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4; //MAX_MOVE_IN_INTERVAL_FAST
/// <summary>
/// 按比例缩放过的在一个更新间隔中的最大移动距离。
/// </summary>
private float _scaledMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL / 1000; //MAX_MOVE_IN_INTERVAL
private float _scaledMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000; //MAX_MOVE_IN_INTERVAL
/// <summary>
/// 速度缩放比例。
@ -76,6 +78,27 @@ namespace Plane.Copters
/// FlyTo 的目标高度。
/// </summary>
private float _targetAlt;
//航点开始高度
private float _startAlt;
private float _Lng_delta;
private float _Lat_delta;
private float _flytime;
private DateTime _startTicks;
private float _distance_xy;
private float _distance_z;
private float _distance_xy_delta;
private float _distance_z_delta;
private float currflytime;
// 根据飞行时间计算飞行距离。
private float currdis_xy;
private float currdis_z;
// 目标点相对于开始位置的方向。
private double _direction;
/// <summary>
/// FlyTo 的目标纬度。
@ -86,6 +109,15 @@ namespace Plane.Copters
/// FlyTo 的目标经度。
/// </summary>
private double _targetLng;
/// <summary>
/// FlyTo 的目标纬度。
/// </summary>
private double _startLat;
/// <summary>
/// FlyTo 的目标经度。
/// </summary>
private double _startLng;
/// <summary>
@ -93,7 +125,7 @@ namespace Plane.Copters
/// </summary>
private bool _ShowLED;
private System.Drawing.ColorConverter rgbconverter;
/// <summary>
@ -101,6 +133,8 @@ namespace Plane.Copters
/// </summary>
public FakeCopter() : this(SynchronizationContext.Current)
{
}
@ -122,7 +156,6 @@ namespace Plane.Copters
}
public Task DoCommandAckAsync(ushort command, byte result)
{
return TaskUtils.CompletedTask;
@ -241,14 +274,37 @@ namespace Plane.Copters
return TaskUtils.CompletedTask;
}
protected override Task FlyToCoreAsync(double lat, double lng, float alt)
protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0)
{
if (!IsEmergencyHoverActive)
{
_targetLat = lat;
_targetLng = lng;
_targetAlt = alt;
_startLat = Latitude;
_startLng = Longitude ;
_startAlt = Altitude;
_direction = this.CalcDirection2D(lat, lng);
_flytime = flytime*1000; //ms
_startTicks = DateTime.Now; //ms
_Lng_delta = (float)( Math.Sin(_direction) * GeographyUtils.CalcMetersToLngSpan(Latitude));
_Lat_delta =(float)( Math.Cos(_direction) * GeographyUtils.METERS_TO_LAT_SPAN);
//计算xy和x方向距离
_distance_xy = (float)this.CalcDistance(lat, lng, Altitude);
_distance_z = alt - Altitude;
Console.WriteLine($"{this.Id}d:{_distance_xy},lat{this.Latitude},lng{this.Longitude },tlat:{lat}tlng:{lng}");
_distance_xy_delta = _distance_xy / _flytime;
_distance_z_delta = _distance_z / _flytime;
Mode = FlightMode.GUIDED;
}
return TaskUtils.CompletedTask;
}
@ -517,6 +573,73 @@ namespace Plane.Copters
}
public static System.Drawing.Color GetRandomColor()
{
Random rand = new Random();
return System.Drawing.Color.FromArgb(rand.Next(256), rand.Next(256), rand.Next(256));
}
private DateTime led_laston;
private System.Drawing.Color Led_color;
//更新显示颜色，根据设置的led参数
private void UpdateShowColor()
{
// 使用实例化的对象调用ConvertFromString
//LEDShowColor = (System.Drawing.Color)converter.ConvertFromString("#" + LEDColor);
// 创建ColorConverter实例用于颜色转换
if ( rgbconverter==null) rgbconverter=new System.Drawing.ColorConverter();
//简化版的颜色模拟
switch (LEDMode)
{
case 0:
if (LEDColor != null)
LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#" + LEDColor);
else LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#FFFFFF");
break;
//闪烁
case 1:
case 2:
case 3:
case 4:
case 5:
case 6:
case 7:
case 8:
case 9:
case 10:
case 11:
case 12:
case 13:
case 14:
if (DateTime.Now.AddMilliseconds(-(LEDInterval*1000)) >= led_laston)
{
led_laston = DateTime.Now;
if (LEDShowColor != System.Drawing.Color.Black)
{
LEDShowColor = System.Drawing.Color.Black;
}
else
{
if ((LEDMode == 1) || (LEDMode == 4) || (LEDMode == 5) || (LEDMode == 6) || (LEDMode == 7) || (LEDMode == 9) || (LEDMode == 11) || (LEDMode == 13))
LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#" + LEDColor);
if ((LEDMode == 2)||(LEDMode == 3) || (LEDMode == 8) || (LEDMode == 10) || (LEDMode == 12) || (LEDMode == 14))
LEDShowColor = GetRandomColor();
}
}
break;
}
}
private bool ReachedDestination(double destLat, double destLng, float destAlt) => this.CalcDistance(destLat, destLng, destAlt) < 0.1;
@ -566,7 +689,11 @@ namespace Plane.Copters
// 王海, 20160317, 指点时也能体感控制若干通道。
//考虑不更新这个，好像没必要-xu
//UpdateWithChannels();
UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
//UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
if (FlightControlMode==1)
UpdateWithDestination_v2(_targetLat, _targetLng, _targetAlt);
else
UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
break;
case FlightMode.STABILIZE:
@ -624,7 +751,7 @@ namespace Plane.Copters
default:
break;
}
UpdateFlightDataIfNeeded();
//UpdateFlightDataIfNeeded();
}
}
else
@ -633,6 +760,11 @@ namespace Plane.Copters
// 锁定时直接把速度设为 0。
AirSpeed = 0;
}
// UpdateShowColor();
_uiSyncContext.Post(() =>
{
//位置变化需要在UI刷新
@ -721,7 +853,7 @@ namespace Plane.Copters
// 更新高度。
Altitude += altDelta;
// 目标点相对于当前位置的方向。
var direction = this.CalcDirection2D(lat, lng);
// var direction = this.CalcDirection2D(lat, lng);
// 水平面上的移动距离。
var moveInHorizontalPlane = Math.Sqrt(move * move - altDelta * altDelta);
if (double.IsNaN(moveInHorizontalPlane))
@ -729,10 +861,48 @@ namespace Plane.Copters
MoveToPointImmediately(lat, lng, alt);
return;
}
// 更新纬度。
Longitude += moveInHorizontalPlane * Math.Sin(direction) * GeographyUtils.CalcMetersToLngSpan(Latitude);
//Longitude += moveInHorizontalPlane * Math.Sin(direction) * GeographyUtils.CalcMetersToLngSpan(Latitude);
Longitude += moveInHorizontalPlane * _Lng_delta;
Latitude += moveInHorizontalPlane * _Lat_delta;
// 更新经度。
Latitude += moveInHorizontalPlane * Math.Cos(direction) * GeographyUtils.METERS_TO_LAT_SPAN;
//Latitude += moveInHorizontalPlane * Math.Cos(direction) * GeographyUtils.METERS_TO_LAT_SPAN;
}
//新版本小航点计算移动位置
private void UpdateWithDestination_v2(double lat, double lng, float alt)
{
//_flytime 总飞行时间 秒
//_startTicks 开始飞行时间ms
// _distance_xy 米
// _distance_z 米
//当前飞行时间
if ((lat == Latitude) && (lng == Longitude) && (alt == Altitude)) return;
currflytime =(float)( DateTime.Now -_startTicks).TotalMilliseconds;//实际飞行时间 ms
//超时直接移动到目标点
if (currflytime >= _flytime)
{
MoveToPointImmediately(lat, lng, alt);
return;
}
//if (currflytime > 13000)
// return;
// 根据飞行时间计算飞行距离
currdis_xy = _distance_xy_delta * currflytime;
currdis_z = _distance_z_delta * currflytime;
// 距离已经很近，直接移动到目标点。
if ((Math.Abs(_distance_xy-currdis_xy) < 0.1)&& (Math.Abs(_distance_z - currdis_z) < 0.1))
{
Console.WriteLine($"{this.Id} to tlat:{lat}tlng:{lng}");
MoveToPointImmediately(lat, lng, alt);
return;
}
// 更新位置
Altitude = _startAlt+ currdis_z;
Longitude = _startLng + currdis_xy*_Lng_delta;
Latitude = _startLat + currdis_xy *_Lat_delta;
}
}
}

2
PlaneGcsSdk_Shared/Copters/PLCopter.cs
View File

@ -418,7 +418,7 @@ namespace Plane.Copters
return !anotherSetModeActionCalled && Mode == mode;
}
protected override async Task FlyToCoreAsync(double lat, double lng, float alt)
protected override async Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0)
{
if (!IsEmergencyHoverActive)
{