group/Plane.Sdk3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
f9814532f5
Plane.Sdk3/PlaneGcsSdk_Shared/Copters/FakeCopter.cs

936 lines
34 KiB
C#
Raw Normal View History

更新使用Vs2022编译 模拟飞机路线计算调整
2023-12-25 18:54:28 +08:00
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;
GC_xy = new GuidController();
GC_z = new GuidController();
/*
// 持续假装收到飞行器发来的心跳。
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; } }
private GuidController GC_xy;
private GuidController GC_z;
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;
GC_xy.initgetspeeddist(_flytype, _flytime / 1000, _distance_xy, acc_xy, maxspeed_xy);
if (_targetAlt>Altitude )
GC_z.initgetspeeddist(_flytype, _flytime / 1000, _distance_z, acc_z, maxspeed_up);
else
GC_z.initgetspeeddist(_flytype, _flytime / 1000, _distance_z, acc_z, maxspeed_down);
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;
GC_xy.getspeeddist(_flytype, _flytime / 1000, _distance_xy, currflytime / 1000, acc_xy,maxspeed_xy , out currdis_xy, out vspeed);
if (alt> Altitude)
GC_z.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_up, out currdis_z, out vspeed);
else
GC_z.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;
}
}
}
Reference in New Issue Copy Permalink