923 lines
33 KiB
C#
923 lines
33 KiB
C#
using Plane.Communication;
|
||
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;
|
||
using Plane.CopterControllers;
|
||
|
||
namespace Plane.Copters
|
||
{
|
||
/// <summary>
|
||
/// 虚拟的 <see cref="ICopter"/> 实现。
|
||
/// </summary>
|
||
[DebuggerDisplay("Name={Name}")]
|
||
public partial class FakeCopter : CopterImplSharedPart, IFakeCopter
|
||
{
|
||
/// <summary>
|
||
/// 心跳间隔，单位为毫秒。
|
||
/// </summary>
|
||
private const int HEARTBEAT_INTERVAL = 200;
|
||
/// <summary>
|
||
/// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
|
||
/// </summary>
|
||
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_TEMP / 1000;
|
||
|
||
/// <summary>
|
||
/// 高速模式下，在一个更新间隔中的最大移动距离。
|
||
/// </summary>
|
||
private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4; //MAX_MOVE_IN_INTERVAL*4
|
||
|
||
|
||
|
||
// private int _UPDATE_INTERVAL = 50;
|
||
|
||
/// <summary>
|
||
/// 对飞行器的模拟是否正在运行。
|
||
/// </summary>
|
||
private bool _isRunning = false;
|
||
|
||
/// <summary>
|
||
/// 上次计算速度时的位置。
|
||
/// </summary>
|
||
private ILocation _lastCalcSpeedPoint;
|
||
|
||
/// <summary>
|
||
/// 上次计算速度的时间。
|
||
/// </summary>
|
||
private DateTime _lastCalcSpeedTime;
|
||
|
||
/// <summary>
|
||
/// 高速模式下，按比例缩放过的在一个更新间隔中的最大移动距离。
|
||
/// </summary>
|
||
private float _scaledFastMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4; //MAX_MOVE_IN_INTERVAL_FAST
|
||
|
||
/// <summary>
|
||
/// 按比例缩放过的在一个更新间隔中的最大移动距离。
|
||
/// </summary>
|
||
private float _scaledMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000; //MAX_MOVE_IN_INTERVAL
|
||
|
||
/// <summary>
|
||
/// 速度缩放比例。
|
||
/// </summary>
|
||
private float _speedScale = 1;
|
||
|
||
/// <summary>
|
||
/// 自动起飞的目标高度。
|
||
/// </summary>
|
||
private int _takeOffTargetAltitude;
|
||
|
||
/// <summary>
|
||
/// 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 _xy_speed;
|
||
private float _z_speed;
|
||
private int _flytype;
|
||
private float currflytime;
|
||
// 根据飞行时间计算飞行距离。
|
||
private float currdis_xy;
|
||
private float currdis_z;
|
||
|
||
|
||
// 目标点相对于开始位置的方向。
|
||
private double _direction;
|
||
|
||
/// <summary>
|
||
/// FlyTo 的目标纬度。
|
||
/// </summary>
|
||
private double _targetLat;
|
||
|
||
/// <summary>
|
||
/// FlyTo 的目标经度。
|
||
/// </summary>
|
||
private double _targetLng;
|
||
/// <summary>
|
||
/// FlyTo 的目标纬度。
|
||
/// </summary>
|
||
private double _startLat;
|
||
|
||
/// <summary>
|
||
/// FlyTo 的目标经度。
|
||
/// </summary>
|
||
private double _startLng;
|
||
|
||
|
||
/// <summary>
|
||
/// FlyTo 的目标经度。
|
||
/// </summary>
|
||
private bool _ShowLED;
|
||
|
||
private System.Drawing.ColorConverter rgbconverter;
|
||
|
||
|
||
/// <summary>
|
||
/// 使用 <see cref="SynchronizationContext.Current"/> 创建 <see cref="FakeCopter"/> 实例。
|
||
/// </summary>
|
||
public FakeCopter() : this(SynchronizationContext.Current)
|
||
{
|
||
|
||
|
||
}
|
||
|
||
|
||
new public int sim_update_int
|
||
{
|
||
get { return UPDATE_INTERVAL; }
|
||
set
|
||
{
|
||
//最大移动距离
|
||
MAX_MOVE_IN_INTERVAL = MAX_VEL * value / 1000;
|
||
//快速模式最大移动距离
|
||
MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4;
|
||
//速度缩放后快速速度距离
|
||
_scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST* _speedScale;
|
||
//速度缩放后速度距离
|
||
_scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL* _speedScale;
|
||
Set(nameof(sim_update_int), ref UPDATE_INTERVAL, value);
|
||
}
|
||
}
|
||
|
||
|
||
public Task DoCommandAckAsync(ushort command, byte result)
|
||
{
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建 <see cref="FakeCopter"/> 实例。
|
||
/// </summary>
|
||
/// <param name="uiSyncContext">UI 线程的同步上下文。</param>
|
||
/// <param name="takeOffTargetAltitude">自动起飞时的目标高度。</param>
|
||
/// <param name="speedScale">速度缩放比例。</param>
|
||
public FakeCopter(SynchronizationContext uiSyncContext, int takeOffTargetAltitude = 10, float speedScale = 1, IConnection connection = null)
|
||
: base(uiSyncContext)
|
||
{
|
||
Latitude = 23.155382266268134;
|
||
Longitude = 113.45038586296141;
|
||
|
||
Elevation = 20;
|
||
this.PropertyChanged += (sender, e) =>
|
||
{
|
||
switch (e.PropertyName)
|
||
{
|
||
case nameof(Altitude):
|
||
Elevation = Altitude + 20;
|
||
break;
|
||
|
||
case nameof(IsUnlocked):
|
||
if (IsUnlocked)
|
||
{
|
||
// 王海, 20151029, 在解锁时上次计算速度点、上次计算速度时刻。
|
||
_lastCalcSpeedPoint = new PLLocation { Latitude = Latitude, Longitude = Longitude, Altitude = Altitude };
|
||
_lastCalcSpeedTime = DateTime.Now;
|
||
}
|
||
break;
|
||
}
|
||
};
|
||
|
||
_takeOffTargetAltitude = takeOffTargetAltitude;
|
||
|
||
_speedScale = speedScale;
|
||
_scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL * _speedScale;
|
||
_scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST * _speedScale;
|
||
|
||
this.Connection = connection ?? EmptyConnection.Instance;
|
||
_isRunning = true;
|
||
// 持续计算并更新虚拟飞行器的状态。
|
||
// Task.Run(async () =>
|
||
|
||
Task.Factory.StartNew(async () =>
|
||
|
||
{
|
||
while (_isRunning)
|
||
{
|
||
Update();
|
||
await TaskUtils.Delay(UPDATE_INTERVAL).ConfigureAwait(false);
|
||
}
|
||
}
|
||
, TaskCreationOptions.LongRunning
|
||
);
|
||
|
||
|
||
++HeartbeatCount;
|
||
GpsFixType = GpsFixType.Fix3D;
|
||
GpsHdop = 1;
|
||
IsGpsAccurate = true;
|
||
HasSwitchedToGpsMode = true;
|
||
|
||
/*
|
||
// 持续假装收到飞行器发来的心跳。
|
||
Task.Run(async () =>
|
||
{
|
||
while (true)
|
||
{
|
||
if (_isRunning)
|
||
{
|
||
++HeartbeatCount;
|
||
|
||
IsCheckingConnection = false;
|
||
|
||
if (HeartbeatCount >= 10)
|
||
{
|
||
// 收到若干个心跳之后，设置一下 GPS 相关属性。
|
||
GpsFixType = GpsFixType.Fix3D;
|
||
GpsHdop = 1;
|
||
IsGpsAccurate = true;
|
||
HasSwitchedToGpsMode = true;
|
||
}
|
||
|
||
_uiSyncContext.Post(() => RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(HeartbeatCount)));
|
||
}
|
||
await TaskUtils.Delay(HEARTBEAT_INTERVAL).ConfigureAwait(false);
|
||
}
|
||
});
|
||
*/
|
||
}
|
||
|
||
public IConnection Connection { get; set; }
|
||
|
||
public string Id { get; set; }
|
||
|
||
private int ReturnToLaunchAltitude { get { return _takeOffTargetAltitude; } }
|
||
|
||
public Task ConnectAsync()
|
||
{
|
||
IsConnected = true;
|
||
_isRunning = true;
|
||
IsCheckingConnection = true;
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
public Task DisconnectAsync()
|
||
{
|
||
IsConnected = false;
|
||
_isRunning = false;
|
||
IsCheckingConnection = false;
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 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}");
|
||
|
||
_flytype = flytype;
|
||
// _xy_speed = _distance_xy / _flytime;
|
||
// _z_speed = _distance_z / _flytime;
|
||
Mode = FlightMode.GUIDED;
|
||
|
||
|
||
}
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
public Task GetCopterDataAsync()
|
||
{
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
public Task<float> GetParamAsync(string paramName, int millisecondsTimeout = Timeout.Infinite)
|
||
{
|
||
// TODO: 王海, 20150806, 实现仿真的 GetParamAsync。
|
||
return Task.FromResult(0f);
|
||
}
|
||
|
||
public Task LockAsync()
|
||
{
|
||
if (IsUnlocked)
|
||
{
|
||
IsUnlocked = false;
|
||
}
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
public Task LEDAsync()
|
||
{
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
public Task MotorTestAsync(int motor)
|
||
{
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
public override Task SetChannelsAsync()
|
||
{
|
||
Channel1 = DesiredChannel1 ?? Channel1;
|
||
Channel2 = DesiredChannel2 ?? Channel2;
|
||
Channel3 = DesiredChannel3 ?? Channel3;
|
||
Channel4 = DesiredChannel4 ?? Channel4;
|
||
Channel5 = DesiredChannel5 ?? Channel5;
|
||
Channel6 = DesiredChannel6 ?? Channel6;
|
||
Channel7 = DesiredChannel7 ?? Channel7;
|
||
Channel8 = DesiredChannel8 ?? Channel8;
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
public override Task SetMobileControlAsync()
|
||
{
|
||
Channel1 = DesiredChannel1 ?? Channel1;
|
||
Channel2 = DesiredChannel2 ?? Channel2;
|
||
Channel3 = DesiredChannel3 ?? Channel3;
|
||
Channel4 = DesiredChannel4 ?? Channel4;
|
||
Channel5 = DesiredChannel5 ?? Channel5;
|
||
Channel6 = DesiredChannel6 ?? Channel6;
|
||
Channel7 = DesiredChannel7 ?? Channel7;
|
||
Channel8 = DesiredChannel8 ?? Channel8;
|
||
Yaw = DesiredYaw ?? Yaw;
|
||
Heading = (short)Yaw;
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
//得到收到的总数据量
|
||
public void GetCommunicationNumber(out int recnumber, out int sendnumber)
|
||
{
|
||
recnumber = 0;
|
||
sendnumber = 0;
|
||
}
|
||
|
||
|
||
//重设数据量
|
||
public void ResetCommunicationNumber()
|
||
{
|
||
|
||
}
|
||
|
||
public async Task DoCommandAsync(int actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
|
||
{
|
||
|
||
await Task.Delay(50).ConfigureAwait(false);
|
||
|
||
|
||
}
|
||
public Task SetParamAsync(string paramName, float value, int millisecondsTimeout = Timeout.Infinite)
|
||
{
|
||
// TODO: 王海, 20150807, 实现仿真的 SetParamAsync。
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
public bool GetShowLEDAsync()
|
||
{
|
||
return _ShowLED;
|
||
}
|
||
public Task SetShowLEDAsync(bool Ledon)
|
||
{
|
||
_ShowLED = Ledon;
|
||
RaiseLocationChanged();
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
|
||
public Task SetShowRGBLEDAsync(int RedValue, int GreenValue, int BlueValue)
|
||
{
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
|
||
public Task SetShowLEDFlashAsync(int Flashtype, int millisecondsTime)
|
||
{
|
||
bool Ledison;
|
||
Ledison = GetShowLEDAsync();
|
||
if (!Ledison)
|
||
{
|
||
SetShowLEDAsync(true);
|
||
Task.Delay(millisecondsTime).ConfigureAwait(false);
|
||
SetShowLEDAsync(false);
|
||
}
|
||
else
|
||
{
|
||
SetShowLEDAsync(false);
|
||
Task.Delay(millisecondsTime).ConfigureAwait(false);
|
||
SetShowLEDAsync(true);
|
||
}
|
||
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
public void SetProperties(
|
||
string id = null, //"Junqing's Drone",
|
||
double? latitude = null, //23.14973333,
|
||
double? longitude = null, //113.40974166,
|
||
float? altitude = null, //0,
|
||
string name = null, //"王海的飞行器",
|
||
byte? batteryPer = null, //10,
|
||
short? heading = null, //33,
|
||
bool? isConnected = null, //true,
|
||
float? pitch = null, //-70,
|
||
float? roll = null, //28,
|
||
byte? satCount = null, //6,
|
||
float? airSpeed = null, //3.333,
|
||
double? flightDistance = null, //100.388,
|
||
double? flightDistance2D = null, // 100.88,
|
||
TimeSpan? flightTimeSpan = null)
|
||
{
|
||
if (id != null) Id = id;
|
||
if (name != null) Name = name;
|
||
if (latitude != null) Latitude = latitude.Value;
|
||
if (longitude != null) Longitude = longitude.Value;
|
||
if (altitude != null) Altitude = altitude.Value;
|
||
if (batteryPer != null) BatteryPer = batteryPer.Value;
|
||
if (heading != null) Heading = heading.Value;
|
||
if (isConnected != null) IsConnected = isConnected.Value;
|
||
if (pitch != null) Pitch = pitch.Value;
|
||
if (roll != null) Roll = roll.Value;
|
||
if (satCount != null) SatCount = satCount.Value;
|
||
if (airSpeed != null) AirSpeed = airSpeed.Value;
|
||
if (flightDistance != null) FlightDistance = flightDistance.Value;
|
||
if (flightDistance2D != null) FlightDistance2D = flightDistance2D.Value;
|
||
if (flightTimeSpan != null) FlightTimeSpan = flightTimeSpan.Value;
|
||
}
|
||
|
||
public Task StartPairingAsync()
|
||
{
|
||
RaisePairingCompleted(new PairingCompletedEventArgs(true, 0, 0));
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
public Task StopPairingAsync()
|
||
{
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
public async Task TakeOffAsync(float alt)
|
||
{
|
||
await UnlockAsync().ConfigureAwait(false);
|
||
_takeOffTargetAltitude = (int)alt;
|
||
await TakeOffAsync().ConfigureAwait(false);
|
||
}
|
||
public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
|
||
{
|
||
return TaskUtils.CompletedTask;
|
||
}
|
||
|
||
|
||
public async Task UnlockAsync()
|
||
{
|
||
if (!IsUnlocked)
|
||
{
|
||
await SetChannelsAsync(
|
||
ch1: 1500,
|
||
ch2: 1500,
|
||
ch3: 1100,
|
||
ch4: 1500
|
||
).ConfigureAwait(false);
|
||
if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false))
|
||
{
|
||
IsUnlocked = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 无论在不在紧急悬停状态下都可用的切换模式方法。
|
||
/// </summary>
|
||
/// <param name="mode">模式。</param>
|
||
/// <param name="millisecondsTimeout">超时。</param>
|
||
/// <returns>成功与否。</returns>
|
||
internal override Task<bool> SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000)
|
||
{
|
||
Mode = mode;
|
||
return Task.FromResult(true);
|
||
}
|
||
|
||
protected override void UpdateFlightDataIfNeeded()
|
||
{
|
||
//计算飞机距离，没必要
|
||
/*
|
||
if (!TakeOffPoint.IsNullOrEmpty())
|
||
{
|
||
FlightDistance = TakeOffPoint.CalcDistance(this);
|
||
FlightDistance2D = TakeOffPoint.CalcDistance2D(this);
|
||
}
|
||
|
||
|
||
if (FlightTimeSpan.TotalSeconds > 0)
|
||
{
|
||
if (_lastCalcSpeedPoint.IsNullOrEmpty())
|
||
{
|
||
if (!this.IsNullOrEmpty())
|
||
{
|
||
_lastCalcSpeedPoint = new PLLocation(this);
|
||
_lastCalcSpeedTime = DateTime.Now;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
var movedDistance = _lastCalcSpeedPoint.CalcDistance(this);
|
||
var movedTime = DateTime.Now - _lastCalcSpeedTime;
|
||
if (movedDistance >= 3 || movedTime.TotalSeconds >= 3)
|
||
{
|
||
var airSpeed = movedDistance / movedTime.TotalSeconds;
|
||
if (airSpeed < 100) // 王海, 20151023, 速度过大时不正常，经纬度可能有错误。
|
||
{
|
||
AirSpeed = (float)airSpeed;
|
||
}
|
||
//Windows.Messages.Message.Show(AirSpeed.ToString());
|
||
GroundSpeed = AirSpeed;
|
||
_lastCalcSpeedPoint = new PLLocation(this);
|
||
_lastCalcSpeedTime = DateTime.Now;
|
||
}
|
||
}
|
||
}
|
||
*/
|
||
}
|
||
|
||
private void MoveToPointImmediately(double lat, double lng, float alt)
|
||
{
|
||
Latitude = lat;
|
||
Longitude = lng;
|
||
Altitude = alt;
|
||
Roll = 0;
|
||
Pitch = 0;
|
||
}
|
||
public Task InjectGpsDataAsync(byte[] data, ushort length)
|
||
{
|
||
return TaskUtils.CompletedTask;
|
||
|
||
}
|
||
|
||
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;
|
||
|
||
/// <summary>
|
||
/// 计算并更新虚拟飞行器状态。
|
||
/// </summary>
|
||
private void Update()
|
||
{
|
||
if (IsUnlocked)
|
||
{
|
||
// 飞行器接触地面（只判断 Altitude <= 0）后，如果处于 LAND 模式或者保持 Channel3 < MIN_HOVER_CHANNEL，在一段时间后自动锁定。
|
||
if ((Mode == FlightMode.LAND || Channel3 < MIN_HOVER_CHANNEL) && Altitude <= 0)
|
||
{
|
||
Task.Run(async () =>
|
||
{
|
||
for (int i = 0; i < 4000 / 20; i++)
|
||
{
|
||
await TaskUtils.Delay(20).ConfigureAwait(false);
|
||
if ((Mode != FlightMode.LAND && Channel3 >= MIN_HOVER_CHANNEL) || !IsUnlocked)
|
||
{
|
||
return;
|
||
}
|
||
}
|
||
IsUnlocked = false;
|
||
});
|
||
}
|
||
else
|
||
{
|
||
switch (Mode)
|
||
{
|
||
case FlightMode.ACRO:
|
||
break;
|
||
|
||
case FlightMode.AUTO:
|
||
/*
|
||
// 暂时只管起飞。以最大速度上升，直到达到目标高度。
|
||
if (Altitude < _takeOffTargetAltitude)
|
||
{
|
||
Altitude += _scaledMaxMoveInInterval;
|
||
}
|
||
else
|
||
{
|
||
Mode = FlightMode.LOITER;
|
||
}
|
||
*/
|
||
break;
|
||
|
||
case FlightMode.GUIDED:
|
||
// 王海, 20160317, 指点时也能体感控制若干通道。
|
||
//考虑不更新这个，好像没必要-xu
|
||
//UpdateWithChannels();
|
||
//UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
|
||
if (FlightControlMode==1)
|
||
UpdateWithDestination_v2(_targetLat, _targetLng, _targetAlt);
|
||
else
|
||
UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
|
||
break;
|
||
|
||
case FlightMode.STABILIZE:
|
||
case FlightMode.ALT_HOLD:
|
||
case FlightMode.LOITER:
|
||
UpdateWithChannels();
|
||
break;
|
||
|
||
case FlightMode.RTL:
|
||
if (Altitude < ReturnToLaunchAltitude && !ReachedDestination(Latitude, Longitude, ReturnToLaunchAltitude))
|
||
{
|
||
UpdateWithDestination(Latitude, Longitude, ReturnToLaunchAltitude);
|
||
}
|
||
else if (Altitude > ReturnToLaunchAltitude && !ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Altitude))
|
||
{
|
||
UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Altitude);
|
||
}
|
||
else
|
||
{
|
||
UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, ReturnToLaunchAltitude);
|
||
}
|
||
break;
|
||
|
||
case FlightMode.CIRCLE:
|
||
break;
|
||
|
||
case FlightMode.POSITION:
|
||
break;
|
||
|
||
case FlightMode.LAND:
|
||
// 王海, 20160317, 降落时也能体感控制若干通道。
|
||
UpdateWithChannels();
|
||
// 以最大速度降落，直到高度为 0。
|
||
if (Altitude > 0)
|
||
{
|
||
if (Altitude > _scaledMaxMoveInInterval)
|
||
{
|
||
Altitude -= _scaledMaxMoveInInterval;
|
||
}
|
||
else
|
||
{
|
||
Altitude = 0;
|
||
}
|
||
Roll = 0;
|
||
Pitch = 0;
|
||
}
|
||
break;
|
||
|
||
case FlightMode.OF_LOITER:
|
||
break;
|
||
|
||
case FlightMode.TOY:
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
//UpdateFlightDataIfNeeded();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// TODO: 王海, 20151228, 模拟空中锁定。
|
||
// 锁定时直接把速度设为 0。
|
||
AirSpeed = 0;
|
||
}
|
||
|
||
// UpdateShowColor();
|
||
|
||
|
||
|
||
_uiSyncContext.Post(() =>
|
||
{
|
||
//位置变化需要在UI刷新
|
||
RaiseLocationChangedIfNeeded();
|
||
//高度变化需要在UI刷新
|
||
// RaiseAltitudeChangedIfNeeded();
|
||
//GPS数据用于显示
|
||
// RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
|
||
//不考虑姿态
|
||
// RaiseAttitudeChanged();
|
||
});
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据各个通道值来更新位置、姿态等状态。
|
||
/// </summary>
|
||
private void UpdateWithChannels()
|
||
{
|
||
var maxMove = Mode == FlightMode.LOITER ? _scaledMaxMoveInInterval : _scaledFastMaxMoveInInterval;
|
||
var ch3Delta = Channel3 > MAX_HOVER_CHANNEL ? Channel3 - MAX_HOVER_CHANNEL : Channel3 < MIN_HOVER_CHANNEL ? Channel3 - MIN_HOVER_CHANNEL : 0;
|
||
if (ch3Delta != 0)
|
||
{
|
||
// 更新相对高度。
|
||
Altitude = Math.Max(0, Altitude + maxMove * ch3Delta / MAX_CHANNEL_DELTA);
|
||
}
|
||
|
||
var yawRad = Yaw.DegToRad();
|
||
var metersToLocalLng = GeographyUtils.CalcMetersToLngSpan(Latitude);
|
||
|
||
var ch1Delta = Channel1 > MAX_HOVER_CHANNEL ? Channel1 - MAX_HOVER_CHANNEL : Channel1 < MIN_HOVER_CHANNEL ? Channel1 - MIN_HOVER_CHANNEL : 0;
|
||
var xDistance = maxMove * ch1Delta / MAX_CHANNEL_DELTA;
|
||
if (ch1Delta != 0)
|
||
{
|
||
// 更新横滚方向的纬度和经度。
|
||
Latitude -= xDistance * Math.Sin(yawRad) * GeographyUtils.METERS_TO_LAT_SPAN;
|
||
Longitude += xDistance * Math.Cos(yawRad) * metersToLocalLng;
|
||
// 更新横滚姿态。
|
||
Roll = MAX_TILT_IN_FLIGHT * ch1Delta / MAX_CHANNEL_DELTA;
|
||
}
|
||
else
|
||
{
|
||
Roll = 0;
|
||
}
|
||
|
||
var ch2Delta = Channel2 > MAX_HOVER_CHANNEL ? Channel2 - MAX_HOVER_CHANNEL : Channel2 < MIN_HOVER_CHANNEL ? Channel2 - MIN_HOVER_CHANNEL : 0;
|
||
var yDistance = maxMove * ch2Delta / MAX_CHANNEL_DELTA;
|
||
if (ch2Delta != 0)
|
||
{
|
||
// 更新俯仰方向的纬度和经度。
|
||
Latitude -= yDistance * Math.Cos(yawRad) * GeographyUtils.METERS_TO_LAT_SPAN;
|
||
Longitude -= yDistance * Math.Sin(yawRad) * metersToLocalLng;
|
||
// 更新俯仰姿态。
|
||
Pitch = MAX_TILT_IN_FLIGHT * ch2Delta / MAX_CHANNEL_DELTA;
|
||
}
|
||
else
|
||
{
|
||
Pitch = 0;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据目的地来更新位置、姿态等状态。
|
||
/// </summary>
|
||
private void UpdateWithDestination(ILocation loc)
|
||
{
|
||
UpdateWithDestination(loc.Latitude, loc.Longitude, loc.Altitude);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据目的地来更新位置、姿态等状态。
|
||
/// </summary>
|
||
private void UpdateWithDestination(double lat, double lng, float alt)
|
||
{
|
||
// 与目标点之间的距离。
|
||
var distance = this.CalcDistance(lat, lng, alt);
|
||
// 距离已经很近，直接移动到目标点。
|
||
if (distance < _scaledMaxMoveInInterval)
|
||
{
|
||
MoveToPointImmediately(lat, lng, alt);
|
||
return;
|
||
}
|
||
// 在空间中的移动距离。
|
||
var move = _scaledMaxMoveInInterval;
|
||
// 竖直方向的移动距离。
|
||
var altDelta = (float)(move * (alt - Altitude) / distance);
|
||
// 更新高度。
|
||
Altitude += altDelta;
|
||
// 目标点相对于当前位置的方向。
|
||
// var direction = this.CalcDirection2D(lat, lng);
|
||
// 水平面上的移动距离。
|
||
var moveInHorizontalPlane = Math.Sqrt(move * move - altDelta * altDelta);
|
||
if (double.IsNaN(moveInHorizontalPlane))
|
||
{
|
||
MoveToPointImmediately(lat, lng, alt);
|
||
return;
|
||
}
|
||
|
||
// 更新纬度。
|
||
//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;
|
||
}
|
||
//新版本小航点计算移动位置
|
||
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;
|
||
// 根据飞行时间计算飞行距离
|
||
|
||
float vspeed = 0;
|
||
|
||
GuidController.getspeeddist(_flytype, _flytime / 1000, _distance_xy, currflytime / 1000, acc_xy,maxspeed_xy , out currdis_xy, out vspeed);
|
||
|
||
if (alt> Altitude)
|
||
GuidController.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_up, out currdis_z, out vspeed);
|
||
else
|
||
GuidController.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_down, out currdis_z, out vspeed);
|
||
|
||
Console.WriteLine($"{this.Id}：time:{currflytime / 1000} to {currdis_z}");
|
||
|
||
// 距离已经很近，直接移动到目标点。
|
||
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;
|
||
}
|
||
|
||
}
|
||
}
|