623 lines
23 KiB
C#
623 lines
23 KiB
C#
|
using Plane.Communication;
|
|||
|
|
using Plane.Geography;
|
|||
|
|
using System;
|
|||
|
|
using System.Diagnostics;
|
|||
|
|
using System.Threading;
|
|||
|
|
using System.Threading.Tasks;
|
|||
|
|
using static Plane.Copters.Constants;
|
|||
|
|
|
|||
|
|
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>
|
|||
|
|
private const float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL / 1000;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// 高速模式下，在一个更新间隔中的最大移动距离。
|
|||
|
|
/// </summary>
|
|||
|
|
private const float MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
|
|||
|
|
/// </summary>
|
|||
|
|
private const int UPDATE_INTERVAL = 100;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// 对飞行器的模拟是否正在运行。
|
|||
|
|
/// </summary>
|
|||
|
|
private bool _isRunning = false;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// 上次计算速度时的位置。
|
|||
|
|
/// </summary>
|
|||
|
|
private ILocation _lastCalcSpeedPoint;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// 上次计算速度的时间。
|
|||
|
|
/// </summary>
|
|||
|
|
private DateTime _lastCalcSpeedTime;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// 高速模式下，按比例缩放过的在一个更新间隔中的最大移动距离。
|
|||
|
|
/// </summary>
|
|||
|
|
private float _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// 按比例缩放过的在一个更新间隔中的最大移动距离。
|
|||
|
|
/// </summary>
|
|||
|
|
private float _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// 速度缩放比例。
|
|||
|
|
/// </summary>
|
|||
|
|
private float _speedScale = 1;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// 自动起飞的目标高度。
|
|||
|
|
/// </summary>
|
|||
|
|
private int _takeOffTargetAltitude;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// FlyTo 的目标高度。
|
|||
|
|
/// </summary>
|
|||
|
|
private float _targetAlt;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// FlyTo 的目标纬度。
|
|||
|
|
/// </summary>
|
|||
|
|
private double _targetLat;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// FlyTo 的目标经度。
|
|||
|
|
/// </summary>
|
|||
|
|
private double _targetLng;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// 使用 <see cref="SynchronizationContext.Current"/> 创建 <see cref="FakeCopter"/> 实例。
|
|||
|
|
/// </summary>
|
|||
|
|
public FakeCopter() : this(SynchronizationContext.Current)
|
|||
|
|
{
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <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;
|
|||
|
|
|
|||
|
|
// 持续计算并更新虚拟飞行器的状态。
|
|||
|
|
Task.Run(async () =>
|
|||
|
|
{
|
|||
|
|
while (true)
|
|||
|
|
{
|
|||
|
|
if (_isRunning)
|
|||
|
|
{
|
|||
|
|
Update();
|
|||
|
|
}
|
|||
|
|
await TaskUtils.Delay(UPDATE_INTERVAL).ConfigureAwait(false);
|
|||
|
|
}
|
|||
|
|
});
|
|||
|
|
|
|||
|
|
// 持续假装收到飞行器发来的心跳。
|
|||
|
|
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)
|
|||
|
|
{
|
|||
|
|
if (!IsEmergencyHoverActive)
|
|||
|
|
{
|
|||
|
|
_targetLat = lat;
|
|||
|
|
_targetLng = lng;
|
|||
|
|
_targetAlt = alt;
|
|||
|
|
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 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 Task SetParamAsync(string paramName, float value, int millisecondsTimeout = Timeout.Infinite)
|
|||
|
|
{
|
|||
|
|
// TODO: 林俊清, 20150807, 实现仿真的 SetParamAsync。
|
|||
|
|
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 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;
|
|||
|
|
}
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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, 指点时也能体感控制若干通道。
|
|||
|
|
UpdateWithChannels();
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
_uiSyncContext.Post(() =>
|
|||
|
|
{
|
|||
|
|
RaiseLocationChangedIfNeeded();
|
|||
|
|
RaiseAltitudeChangedIfNeeded();
|
|||
|
|
|
|||
|
|
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);
|
|||
|
|
|
|||
|
|
// 更新姿态。
|
|||
|
|
if (Mode == FlightMode.RTL)
|
|||
|
|
{
|
|||
|
|
// 林俊清, 20160126, 目前飞行器只在返航时会旋转机头，面对飞行方向。
|
|||
|
|
Yaw = (float)direction.RadToDeg();
|
|||
|
|
Heading = (short)Yaw;
|
|||
|
|
Roll = 0;
|
|||
|
|
Pitch = MAX_TILT_IN_FLIGHT;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
var directionDelta = direction - Heading.DegToRad();
|
|||
|
|
Roll = MAX_TILT_IN_FLIGHT * (float)Math.Sin(directionDelta);
|
|||
|
|
Pitch = MAX_TILT_IN_FLIGHT * (float)Math.Cos(directionDelta);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 水平面上的移动距离。
|
|||
|
|
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);
|
|||
|
|
// 更新经度。
|
|||
|
|
Latitude += moveInHorizontalPlane * Math.Cos(direction) * GeographyUtils.METERS_TO_LAT_SPAN;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|