1329 lines
44 KiB
C#
1329 lines
44 KiB
C#
using Plane.Geography;
|
||
using System;
|
||
using System.ComponentModel;
|
||
using System.Threading;
|
||
using System.Threading.Tasks;
|
||
|
||
namespace Plane.Copters
|
||
{
|
||
public abstract partial class CopterImplSharedPart : PLObservableObject, ILocation
|
||
{
|
||
protected CopterState? _forcedState;
|
||
|
||
protected bool _isMobileControlActive;
|
||
|
||
protected float? _lastChangedAlt;
|
||
|
||
protected ILocation _lastChangedLocation;
|
||
|
||
protected DateTime _lastRaiseAltitudeChangedTime;
|
||
|
||
protected DateTime _lastRaiseLocationChangedTime;
|
||
|
||
protected bool _shouldFollow;
|
||
|
||
protected DateTime? _takeOffTime;
|
||
|
||
/// <summary>
|
||
/// 更新 <see cref="FlightTimeSpan"/> 的时间间隔。
|
||
/// </summary>
|
||
private static readonly TimeSpan _intervalToUpdateFlightTimeSpan = TimeSpan.FromSeconds(0.5);
|
||
|
||
private float _followAltDifference;
|
||
|
||
private float _followDistance;
|
||
|
||
private bool _followKeepFacingTarget;
|
||
|
||
private bool _followKeep3DRelativeLocations;
|
||
|
||
private bool _followKeepYawDifference;
|
||
|
||
private ILocation _followLastDestination;
|
||
|
||
private IVisibleStatus _followTarget;
|
||
|
||
private float _followSelfDirectionFromTarget;
|
||
|
||
private float _followTargetOriginalYaw;
|
||
|
||
private float? _followYaw;
|
||
|
||
private DateTime _lastUpdateStatusTextTime;
|
||
|
||
private int _mobileControlIntervalMilli = 50;
|
||
|
||
#region Backing Fields
|
||
|
||
private float _AirSpeed;
|
||
|
||
private float _Altitude;
|
||
|
||
private byte _BatteryPer = 100;
|
||
|
||
private ushort _Channel1;
|
||
|
||
private ushort _Channel2;
|
||
|
||
private ushort _Channel3;
|
||
|
||
private ushort _Channel4;
|
||
|
||
private ushort _Channel5;
|
||
|
||
private ushort _Channel6;
|
||
|
||
private ushort _Channel7;
|
||
|
||
private ushort _Channel8;
|
||
|
||
private ushort? _DesiredChannel1;
|
||
|
||
private ushort? _DesiredChannel2;
|
||
|
||
private ushort? _DesiredChannel3;
|
||
|
||
private ushort? _DesiredChannel4;
|
||
|
||
private ushort? _DesiredChannel5;
|
||
|
||
private ushort? _DesiredChannel6;
|
||
|
||
private ushort? _DesiredChannel7;
|
||
|
||
private ushort? _DesiredChannel8;
|
||
|
||
private float? _DesiredYaw;
|
||
|
||
private float _Elevation;
|
||
|
||
private int? _FirmwareVersion;
|
||
|
||
private string _FirmwareVersionText;
|
||
|
||
private double _FlightDistance;
|
||
|
||
private double _FlightDistance2D;
|
||
|
||
private TimeSpan _FlightTime;
|
||
|
||
private GpsFixType _GpsFixType;
|
||
|
||
private float _GpsHdop;
|
||
|
||
private float _GroundSpeed;
|
||
|
||
private bool _HasSwitchedToGpsMode;
|
||
|
||
private short _Heading;
|
||
|
||
private ulong _HeartbeatCount;
|
||
|
||
private bool _IsAbsolutelyConnected;
|
||
|
||
private bool _IsCheckingConnection;
|
||
|
||
private bool _CommModuleConnected;
|
||
|
||
private bool _IsConnected;
|
||
|
||
private bool _IsEmergencyHoverActive;
|
||
|
||
private bool _IsGpsAccurate;
|
||
|
||
private bool _IsPairing;
|
||
|
||
private bool _IsUnlocked;
|
||
|
||
private double _Latitude;
|
||
|
||
private double _Longitude;
|
||
|
||
private FlightMode _Mode;
|
||
private FlightMode _CommModuleMode;
|
||
|
||
private float _Pitch;
|
||
|
||
private float _Roll;
|
||
|
||
private byte _SatCount;
|
||
|
||
private byte[] _Retain = new byte[4];
|
||
|
||
private float _GroundAlt = 0;
|
||
|
||
private CopterState _State;
|
||
|
||
private string _StatusText;
|
||
|
||
private ILocation _TakeOffPoint;
|
||
|
||
private uint _TimebootMs;
|
||
|
||
private float _Voltage;
|
||
|
||
private float _Yaw;
|
||
|
||
//初始时位置：使用地图Center
|
||
private double _RecordLat;
|
||
|
||
private double _RecordLng;
|
||
|
||
|
||
private int _VirtualId;
|
||
|
||
private bool _DisplayVirtualId = false;
|
||
|
||
private bool _DisplayID = true;
|
||
|
||
#endregion Backing Fields
|
||
|
||
public CopterImplSharedPart(SynchronizationContext uiSyncContext) : base(uiSyncContext)
|
||
{
|
||
PropertyChanged += CopterImplSharedPart_PropertyChanged;
|
||
/*
|
||
Task.Run(async () =>
|
||
{
|
||
while (true)
|
||
{
|
||
UpdateFlightTimeSpanIfNeeded();
|
||
await Task.Delay(_intervalToUpdateFlightTimeSpan).ConfigureAwait(false);
|
||
}
|
||
});
|
||
|
||
Task.Run(async () =>
|
||
{
|
||
while (true)
|
||
{
|
||
if (!_isMobileControlActive)
|
||
{
|
||
await EnsureChannelsSetAsync();
|
||
}
|
||
if (_lastUpdateStatusTextTime.AddSeconds(2) <= DateTime.Now)
|
||
{
|
||
StatusText = null;
|
||
}
|
||
await Task.Delay(500); //50
|
||
}
|
||
});
|
||
|
||
Task.Run(async () =>
|
||
{
|
||
var lastHeartbeatCount = HeartbeatCount;
|
||
while (true)
|
||
{
|
||
if (!IsConnected || lastHeartbeatCount == HeartbeatCount)
|
||
{
|
||
// 如果 !IsConnected（连接通道断开），肯定失联了；
|
||
// 或者，过了一段时间，心跳数没变化，认为飞行器失联。
|
||
IsAbsolutelyConnected = false;
|
||
|
||
// 此处等待的时间较短，目的是在飞行器重新连上时快速知晓。
|
||
await Task.Delay(500).ConfigureAwait(false);
|
||
}
|
||
else
|
||
{
|
||
// 过了一段时间，心跳数变化了，确认飞行器连接正常。
|
||
IsAbsolutelyConnected = true;
|
||
|
||
// 记录此时的心跳数。
|
||
lastHeartbeatCount = HeartbeatCount;
|
||
|
||
// 此处等待的时间较长，目的是提高失联判断的可信程度。
|
||
await Task.Delay(5000).ConfigureAwait(false);
|
||
}
|
||
}
|
||
|
||
});
|
||
*/
|
||
}
|
||
|
||
public event EventHandler AltitudeChanged;
|
||
|
||
public event EventHandler AttitudeChanged;
|
||
|
||
#if PRIVATE
|
||
public event EventHandler<DataStreamReceivedEventArgs> DataStreamReceived;
|
||
#endif
|
||
|
||
public event EventHandler<HeartbeatReceivedEventArgs> HeartbeatReceived;
|
||
|
||
public event EventHandler LocationChanged;
|
||
|
||
public event EventHandler<MissionItemReceivedEventArgs> MissionItemReceived;
|
||
|
||
public event EventHandler<PairingCompletedEventArgs> PairingCompleted;
|
||
|
||
public event EventHandler SensorDataReceived;
|
||
|
||
public event EventHandler<SystemStatusReceivedEventArgs> SystemStatusReceived;
|
||
|
||
public event EventHandler<MessageCreatedEventArgs> TextReceived;
|
||
|
||
|
||
protected enum PDataStreamType
|
||
{
|
||
SYS_STATUS = 1, // MAVLINK_MSG_ID_SYS_STATUS,
|
||
GPS_RAW_INT = 24, // MAVLINK_MSG_ID_GPS_RAW_INT,
|
||
RC_CHANNELS_RAW = 35 // MAVLINK_MSG_ID_RC_CHANNELS_RAW
|
||
}
|
||
|
||
public float AirSpeed
|
||
{
|
||
get { return _AirSpeed; }
|
||
protected set { Set(nameof(AirSpeed), ref _AirSpeed, value); }
|
||
}
|
||
|
||
public float Altitude
|
||
{
|
||
get { return _Altitude; }
|
||
protected set { Set(nameof(Altitude), ref _Altitude, value); }
|
||
}
|
||
|
||
public byte BatteryPer
|
||
{
|
||
get { return _BatteryPer; }
|
||
protected set { Set(nameof(BatteryPer), ref _BatteryPer, value); }
|
||
}
|
||
|
||
public ushort Channel1
|
||
{
|
||
get { return _Channel1; }
|
||
protected set { Set(nameof(Channel1), ref _Channel1, value); }
|
||
}
|
||
|
||
public ushort Channel2
|
||
{
|
||
get { return _Channel2; }
|
||
protected set { Set(nameof(Channel2), ref _Channel2, value); }
|
||
}
|
||
|
||
public ushort Channel3
|
||
{
|
||
get { return _Channel3; }
|
||
protected set { Set(nameof(Channel3), ref _Channel3, value); }
|
||
}
|
||
|
||
public ushort Channel4
|
||
{
|
||
get { return _Channel4; }
|
||
protected set { Set(nameof(Channel4), ref _Channel4, value); }
|
||
}
|
||
|
||
public ushort Channel5
|
||
{
|
||
get { return _Channel5; }
|
||
protected set { Set(nameof(Channel5), ref _Channel5, value); }
|
||
}
|
||
|
||
public ushort Channel6
|
||
{
|
||
get { return _Channel6; }
|
||
protected set { Set(nameof(Channel6), ref _Channel6, value); }
|
||
}
|
||
|
||
public ushort Channel7
|
||
{
|
||
get { return _Channel7; }
|
||
protected set { Set(nameof(Channel7), ref _Channel7, value); }
|
||
}
|
||
|
||
public ushort Channel8
|
||
{
|
||
get { return _Channel8; }
|
||
protected set { Set(nameof(Channel8), ref _Channel8, value); }
|
||
}
|
||
|
||
public ushort? DesiredChannel1
|
||
{
|
||
get { return _DesiredChannel1; }
|
||
set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel1), ref _DesiredChannel1, value); } }
|
||
}
|
||
|
||
public ushort? DesiredChannel2
|
||
{
|
||
get { return _DesiredChannel2; }
|
||
set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel2), ref _DesiredChannel2, value); } }
|
||
}
|
||
|
||
public ushort? DesiredChannel3
|
||
{
|
||
get { return _DesiredChannel3; }
|
||
set
|
||
{
|
||
// 王海, 20160317, 紧急悬停时可调节高度。
|
||
//if (!IsEmergencyHoverActive)
|
||
//{
|
||
if (Set(nameof(DesiredChannel3), ref _DesiredChannel3, value))
|
||
{
|
||
if (State == CopterState.Initialized) SetStateAccordingToMode(Mode);
|
||
}
|
||
//}
|
||
}
|
||
}
|
||
|
||
public ushort? DesiredChannel4
|
||
{
|
||
get { return _DesiredChannel4; }
|
||
set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel4), ref _DesiredChannel4, value); } }
|
||
}
|
||
|
||
public ushort? DesiredChannel5
|
||
{
|
||
get { return _DesiredChannel5; }
|
||
set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel5), ref _DesiredChannel5, value); } }
|
||
}
|
||
|
||
public ushort? DesiredChannel6
|
||
{
|
||
get { return _DesiredChannel6; }
|
||
set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel6), ref _DesiredChannel6, value); } }
|
||
}
|
||
|
||
public ushort? DesiredChannel7
|
||
{
|
||
get { return _DesiredChannel7; }
|
||
set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel7), ref _DesiredChannel7, value); } }
|
||
}
|
||
|
||
public ushort? DesiredChannel8
|
||
{
|
||
get { return _DesiredChannel8; }
|
||
set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel8), ref _DesiredChannel8, value); } }
|
||
}
|
||
|
||
public float? DesiredYaw
|
||
{
|
||
get { return _DesiredYaw; }
|
||
set { if (!IsEmergencyHoverActive && !(State == CopterState.Following && _followKeepFacingTarget)) { Set(nameof(DesiredYaw), ref _DesiredYaw, value); } }
|
||
}
|
||
|
||
public float Elevation
|
||
{
|
||
get { return _Elevation; }
|
||
protected set { Set(nameof(Elevation), ref _Elevation, value); }
|
||
}
|
||
|
||
public int VirtualId
|
||
{
|
||
get { return _VirtualId; }
|
||
set { Set(nameof(VirtualId), ref _VirtualId, value); RefreashLoc(); }
|
||
}
|
||
|
||
public bool DisplayVirtualId
|
||
{
|
||
get { return _DisplayVirtualId; }
|
||
set { Set(nameof(DisplayVirtualId), ref _DisplayVirtualId, value); RefreashLoc(); }
|
||
}
|
||
|
||
public bool DisplayID
|
||
{
|
||
get { return _DisplayID; }
|
||
set { Set(nameof(DisplayID), ref _DisplayID, value); RefreashLoc(); }
|
||
}
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// 获取固件版本。
|
||
/// </summary>
|
||
public int? FirmwareVersion
|
||
{
|
||
get { return _FirmwareVersion; }
|
||
protected set
|
||
{
|
||
if (Set(nameof(FirmwareVersion), ref _FirmwareVersion, value))
|
||
{
|
||
if (value == null)
|
||
{
|
||
FirmwareVersionText = null;
|
||
}
|
||
else
|
||
{
|
||
int major, minor, build;
|
||
int temp;
|
||
#if NETFX_CORE
|
||
major = value.Value / 0x1000;
|
||
temp = value.Value % 0x1000;
|
||
minor = temp / 0x100;
|
||
build = temp % 0x100;
|
||
#else
|
||
major = Math.DivRem(value.Value, 0x1000, out temp);
|
||
minor = Math.DivRem(temp, 0x100, out build);
|
||
#endif
|
||
FirmwareVersionText = $"{major}.{minor}.{build}";
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取固件版本的文本形式。
|
||
/// </summary>
|
||
public string FirmwareVersionText
|
||
{
|
||
get { return _FirmwareVersionText; }
|
||
protected set { Set(nameof(FirmwareVersionText), ref _FirmwareVersionText, value); }
|
||
}
|
||
|
||
public double FlightDistance
|
||
{
|
||
get { return _FlightDistance; }
|
||
protected set { Set(nameof(FlightDistance), ref _FlightDistance, value); }
|
||
}
|
||
|
||
public double FlightDistance2D
|
||
{
|
||
get { return _FlightDistance2D; }
|
||
protected set { Set(nameof(FlightDistance2D), ref _FlightDistance2D, value); }
|
||
}
|
||
|
||
public TimeSpan FlightTimeSpan
|
||
{
|
||
get { return _FlightTime; }
|
||
protected set { Set(nameof(FlightTimeSpan), ref _FlightTime, value); }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取 GPS 锁定状态。
|
||
/// </summary>
|
||
public GpsFixType GpsFixType
|
||
{
|
||
get { return _GpsFixType; }
|
||
protected set { Set(nameof(GpsFixType), ref _GpsFixType, value); }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取 GPS HDOP（horizontal dilution of position），单位为米。
|
||
/// </summary>
|
||
public float GpsHdop
|
||
{
|
||
get { return _GpsHdop; }
|
||
protected set { Set(nameof(GpsHdop), ref _GpsHdop, value); }
|
||
}
|
||
|
||
public float GroundSpeed
|
||
{
|
||
get { return _GroundSpeed; }
|
||
protected set { Set(nameof(GroundSpeed), ref _GroundSpeed, value); }
|
||
}
|
||
|
||
public bool HasSwitchedToGpsMode
|
||
{
|
||
get { return _HasSwitchedToGpsMode; }
|
||
protected set { Set(nameof(HasSwitchedToGpsMode), ref _HasSwitchedToGpsMode, value); }
|
||
}
|
||
|
||
public short Heading
|
||
{
|
||
get { return _Heading; }
|
||
protected set { Set(nameof(Heading), ref _Heading, value); }
|
||
}
|
||
|
||
public ulong HeartbeatCount
|
||
{
|
||
get { return _HeartbeatCount; }
|
||
protected set { Set(nameof(HeartbeatCount), ref _HeartbeatCount, value); }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取一个值，指示是否确定飞行器与地面站正常连接。在实现中，一般根据对心跳包的接收情况判断。
|
||
/// </summary>
|
||
public bool IsAbsolutelyConnected
|
||
{
|
||
get { return _IsAbsolutelyConnected; }
|
||
protected set { Set(nameof(IsAbsolutelyConnected), ref _IsAbsolutelyConnected, value); }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取或设置一个值，指示是否正在检查飞行器与地面站的连接以确定通信正常。在实现中，一般在建立连接后设为 true，在收到飞行器发回的数据后设为 false。
|
||
/// </summary>
|
||
public bool IsCheckingConnection
|
||
{
|
||
get { return _IsCheckingConnection; }
|
||
set { Set(nameof(IsCheckingConnection), ref _IsCheckingConnection, value); }
|
||
}
|
||
|
||
public bool IsConnected
|
||
{
|
||
get { return _IsConnected; }
|
||
protected set { Set(nameof(IsConnected), ref _IsConnected, value); }
|
||
}
|
||
|
||
public bool CommModuleConnected
|
||
{
|
||
get { return _CommModuleConnected; }
|
||
set { Set(nameof(CommModuleConnected), ref _CommModuleConnected, value); }
|
||
}
|
||
|
||
public bool IsEmergencyHoverActive
|
||
{
|
||
get { return _IsEmergencyHoverActive; }
|
||
protected set { Set(nameof(IsEmergencyHoverActive), ref _IsEmergencyHoverActive, value); }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取一个值，指示当前的 GPS 定位是否足够精确。
|
||
/// </summary>
|
||
public bool IsGpsAccurate
|
||
{
|
||
get { return _IsGpsAccurate; }
|
||
protected set { Set(nameof(IsGpsAccurate), ref _IsGpsAccurate, value); }
|
||
}
|
||
|
||
public bool IsPairing
|
||
{
|
||
get { return _IsPairing; }
|
||
protected set { Set(nameof(IsPairing), ref _IsPairing, value); }
|
||
}
|
||
|
||
public bool IsUnlocked
|
||
{
|
||
get { return _IsUnlocked; }
|
||
protected set
|
||
{
|
||
if (Set(nameof(IsUnlocked), ref _IsUnlocked, value))
|
||
{
|
||
if (value)
|
||
{
|
||
State = CopterState.Initialized;
|
||
}
|
||
else
|
||
{
|
||
State = CopterState.Locked;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
public double Latitude
|
||
{
|
||
get { return _Latitude; }
|
||
protected set { Set(nameof(Latitude), ref _Latitude, value); }
|
||
}
|
||
|
||
public double Longitude
|
||
{
|
||
get { return _Longitude; }
|
||
protected set { Set(nameof(Longitude), ref _Longitude, value); }
|
||
}
|
||
|
||
public double RecordLat
|
||
{
|
||
get { return _RecordLat; }
|
||
set { Set(nameof(RecordLat), ref _RecordLat, value); }
|
||
}
|
||
|
||
public double RecordLng
|
||
{
|
||
get { return _RecordLng; }
|
||
set { Set(nameof(RecordLng), ref _RecordLng, value); }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取任务总数。
|
||
/// </summary>
|
||
public ushort? MissionCount { get; protected set; }
|
||
|
||
public string Name { get; set; }
|
||
|
||
public float Pitch
|
||
{
|
||
get { return _Pitch; }
|
||
protected set { Set(nameof(Pitch), ref _Pitch, value); }
|
||
}
|
||
|
||
public float Roll
|
||
{
|
||
get { return _Roll; }
|
||
protected set { Set(nameof(Roll), ref _Roll, value); }
|
||
}
|
||
|
||
public byte SatCount
|
||
{
|
||
get { return _SatCount; }
|
||
protected set { Set(nameof(SatCount), ref _SatCount, value); }
|
||
}
|
||
|
||
public float RetainInt
|
||
{
|
||
get { return BitConverter.ToSingle(Retain,0); }
|
||
}
|
||
|
||
public float GroundAlt
|
||
{
|
||
get { return _GroundAlt; }
|
||
set {
|
||
Set(nameof(GroundAlt), ref _GroundAlt, value);
|
||
RaisePropertyChanged(nameof(GroundAlt));
|
||
}
|
||
}
|
||
|
||
|
||
public byte[] Retain
|
||
{
|
||
get { return _Retain; }
|
||
protected set
|
||
{
|
||
Set(nameof(Retain), ref _Retain, value);
|
||
RaisePropertyChanged(nameof(RetainInt));
|
||
}
|
||
}
|
||
|
||
public CopterState State
|
||
{
|
||
get { return _State; }
|
||
protected set { Set(nameof(State), ref _State, _forcedState ?? value); }
|
||
}
|
||
|
||
public string StatusText
|
||
{
|
||
get { return _StatusText; }
|
||
protected set
|
||
{
|
||
if (Set(nameof(StatusText), ref _StatusText, value))
|
||
{
|
||
_lastUpdateStatusTextTime = DateTime.Now;
|
||
}
|
||
}
|
||
}
|
||
|
||
public ILocation TakeOffPoint
|
||
{
|
||
get { return _TakeOffPoint; }
|
||
protected set { Set(nameof(TakeOffPoint), ref _TakeOffPoint, value); }
|
||
}
|
||
|
||
public float Voltage
|
||
{
|
||
get { return _Voltage; }
|
||
protected set { Set(nameof(Voltage), ref _Voltage, value); }
|
||
}
|
||
|
||
public float Yaw
|
||
{
|
||
get { return _Yaw; }
|
||
protected set { Set(nameof(Yaw), ref _Yaw, value); }
|
||
}
|
||
|
||
public FlightMode CommModuleMode
|
||
{
|
||
get { return _CommModuleMode; }
|
||
protected set { Set(nameof(CommModuleMode), ref _CommModuleMode, value); }
|
||
}
|
||
|
||
private byte _CommModuleVersion;
|
||
public byte CommModuleVersion
|
||
{
|
||
get { return _CommModuleVersion; }
|
||
protected set { Set(nameof(CommModuleVersion), ref _CommModuleVersion, value); }
|
||
}
|
||
|
||
private string _LEDColor;
|
||
public string LEDColor
|
||
{
|
||
get { return _LEDColor; }
|
||
set {
|
||
if (Set(nameof(LEDColor), ref _LEDColor, value))
|
||
//强制刷新颜色--在刷新位置时才刷新颜色
|
||
RefreashLoc();
|
||
}
|
||
}
|
||
|
||
#if PRIVATE
|
||
public
|
||
#else
|
||
internal
|
||
#endif
|
||
FlightMode Mode
|
||
{
|
||
get { return _Mode; }
|
||
set
|
||
{
|
||
// 王海, 20160325, Mode 属性改成 internal 了，如果仍然用 Set 或者 RaisePropertyChanged 方法，会抛异常（"Property not found"）。现决定不引发 PropertyChanged 事件了。
|
||
var changed = _Mode != value;
|
||
if (changed) _Mode = value;
|
||
|
||
// 如果在已解锁状态下满足某些条件，就根据模式设置 State。
|
||
if (IsUnlocked)
|
||
{
|
||
var shouldSetState = false;
|
||
if (changed)
|
||
{
|
||
if (State == CopterState.Following)
|
||
{
|
||
if (value != FlightMode.GUIDED)
|
||
{
|
||
// 跟随时切换到其他模式，停止跟随并且设置 State。
|
||
_shouldFollow = false;
|
||
shouldSetState = true;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 非跟随时模式变化了，设置 State。
|
||
shouldSetState = true;
|
||
}
|
||
}
|
||
if (shouldSetState) SetStateAccordingToMode(value, defaultState: CopterState.Initialized);
|
||
}
|
||
}
|
||
}
|
||
|
||
public async Task FloatAsync()
|
||
{
|
||
if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false))
|
||
{
|
||
await SetChannelsAsync(
|
||
ch1: 1500,
|
||
ch2: 1500,
|
||
ch3: 1500,
|
||
ch4: 1500
|
||
).ConfigureAwait(false);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使飞行器飞往水平面上指定的点。
|
||
/// </summary>
|
||
/// <param name="lat">纬度。</param>
|
||
/// <param name="lng">经度。</param>
|
||
/// <returns>表示此命令异步发送操作的 <see cref="Task"/> 实例。</returns>
|
||
public Task FlyToAsync(double lat, double lng)
|
||
{
|
||
return FlyToAsync(lat, lng, Altitude);
|
||
}
|
||
|
||
public Task FlyToAsync(double lat, double lng, float alt)
|
||
{
|
||
_shouldFollow = false;
|
||
//State = CopterState.CommandMode;
|
||
return FlyToCoreAsync(lat, lng, alt);
|
||
}
|
||
|
||
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()
|
||
{
|
||
if (await SetModeAsync(FlightMode.LOITER).ConfigureAwait(false))
|
||
{
|
||
await SetChannelsAsync(
|
||
ch1: 1500,
|
||
ch2: 1500,
|
||
ch3: 1500,
|
||
ch4: 1500
|
||
).ConfigureAwait(false);
|
||
}
|
||
}
|
||
public async Task GuidAsync()
|
||
{
|
||
if (await SetModeAsync(FlightMode.GUIDED).ConfigureAwait(false))
|
||
{
|
||
await SetChannelsAsync(
|
||
ch1: 1500,
|
||
ch2: 1500,
|
||
ch3: 1500,
|
||
ch4: 1500
|
||
).ConfigureAwait(false);
|
||
}
|
||
}
|
||
|
||
public async Task LandAsync()
|
||
{
|
||
await SetModeAsync(FlightMode.LAND).ConfigureAwait(false);
|
||
await SetChannelsAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task ReturnToLaunchAsync()
|
||
{
|
||
await SetModeAsync(FlightMode.RTL).ConfigureAwait(false);
|
||
await SetChannelsAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task SetChannelsAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null)
|
||
{
|
||
SetTargets(ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8);
|
||
|
||
await SetChannelsAsync().ConfigureAwait(false);
|
||
}
|
||
|
||
public abstract Task SetChannelsAsync();
|
||
|
||
public async Task SetMobileControlAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null) // 王海, 20150912, 将来如有需要再补上 TargetAlt, float? alt = null)
|
||
{
|
||
SetTargets(ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, yaw);
|
||
|
||
await SetMobileControlAsync().ConfigureAwait(false);
|
||
}
|
||
|
||
public abstract Task SetMobileControlAsync();
|
||
|
||
public async Task StartEmergencyHoverAsync()
|
||
{
|
||
_shouldFollow = false;
|
||
State = CopterState.HoverMode;
|
||
IsEmergencyHoverActive = true;
|
||
|
||
// 王海, 20151028, 先 IsEmergencyHoverActive = true 再设置目标通道是为了防止别的线程正好在 IsEmergencyHoverActive == false 时设置通道。
|
||
// 王海, 20151019, 在紧急悬停状态，外部设置 DesiredChannel[1-4] 无效，只能如此设置。
|
||
SetFieldAndRaisePropertyChanged(ref _DesiredChannel1, 1500, nameof(DesiredChannel1));
|
||
SetFieldAndRaisePropertyChanged(ref _DesiredChannel2, 1500, nameof(DesiredChannel2));
|
||
SetFieldAndRaisePropertyChanged(ref _DesiredChannel3, 1500, nameof(DesiredChannel3));
|
||
SetFieldAndRaisePropertyChanged(ref _DesiredChannel4, 1500, nameof(DesiredChannel4));
|
||
|
||
await SetMobileControlAsync().ConfigureAwait(false);
|
||
|
||
switch (Mode)
|
||
{
|
||
// 王海, 20151019, 波子说一律切 LOITER。
|
||
//case FlightMode.ALT_HOLD:
|
||
// break;
|
||
//case FlightMode.LOITER:
|
||
// break;
|
||
//case FlightMode.STABILIZE:
|
||
// await SetModeCoreAsync(FlightMode.ALT_HOLD).ConfigureAwait(false);
|
||
// break;
|
||
//case FlightMode.AUTO:
|
||
//case FlightMode.GUIDED:
|
||
//case FlightMode.RTL:
|
||
//case FlightMode.CIRCLE:
|
||
//case FlightMode.LAND:
|
||
default:
|
||
await SetModeCoreAsync(FlightMode.LOITER).ConfigureAwait(false);
|
||
break;
|
||
}
|
||
}
|
||
|
||
public async void StartMobileControl(int? millisecondsInterval = null)
|
||
{
|
||
if (millisecondsInterval != null) _mobileControlIntervalMilli = millisecondsInterval.Value;
|
||
if (!_isMobileControlActive)
|
||
{
|
||
_isMobileControlActive = true;
|
||
while (_isMobileControlActive)
|
||
{
|
||
if (State == CopterState.Following)
|
||
{
|
||
await SetMobileControlAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false);
|
||
}
|
||
else {
|
||
switch (Mode)
|
||
{
|
||
case FlightMode.STABILIZE:
|
||
case FlightMode.ALT_HOLD:
|
||
case FlightMode.LOITER:
|
||
await SetMobileControlAsync().ConfigureAwait(false);
|
||
break;
|
||
|
||
case FlightMode.GUIDED:
|
||
case FlightMode.RTL:
|
||
await SetMobileControlAsync(ch1: 1500, ch2: 1500, ch3: 1500).ConfigureAwait(false);
|
||
break;
|
||
|
||
case FlightMode.LAND:
|
||
await SetMobileControlAsync(ch3: 1500);
|
||
break;
|
||
|
||
default:
|
||
await SetMobileControlAsync(ch1: 1500, ch2: 1500, yaw: Yaw).ConfigureAwait(false);
|
||
break;
|
||
}
|
||
}
|
||
await Task.Delay(_mobileControlIntervalMilli).ConfigureAwait(false);
|
||
}
|
||
}
|
||
}
|
||
|
||
public void StopEmergencyHover()
|
||
{
|
||
IsEmergencyHoverActive = false;
|
||
}
|
||
|
||
public void StopMobileControl()
|
||
{
|
||
_isMobileControlActive = false;
|
||
}
|
||
|
||
public async Task TakeOffAsync()
|
||
{
|
||
|
||
|
||
/* await SetChannelsAsync(
|
||
ch1: 1500,
|
||
ch2: 1500,
|
||
ch3: 1500,
|
||
ch4: 1500
|
||
).ConfigureAwait(false);
|
||
_forcedState = CopterState.TakingOff;
|
||
State = CopterState.TakingOff;
|
||
await DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, alt).ConfigureAwait(false);
|
||
var temp = Task.Run(async () =>
|
||
{
|
||
var startTime = DateTime.Now;
|
||
var secondsTimeout = alt / 0.7;
|
||
while (currentTakeOffCount == _takeOffCount)
|
||
{
|
||
if (alt - Altitude < 0.2 || startTime.AddSeconds(secondsTimeout) <= DateTime.Now)
|
||
{
|
||
_forcedState = null;
|
||
await HoverAsync().ConfigureAwait(false);
|
||
break;
|
||
}
|
||
await Task.Delay(50).ConfigureAwait(false);
|
||
}
|
||
});
|
||
*/
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
if (await SetModeAsync(FlightMode.AUTO).ConfigureAwait(false))
|
||
{
|
||
await SetChannelsAsync(
|
||
ch1: 1500,
|
||
ch2: 1500,
|
||
ch3: 1500,
|
||
ch4: 1500
|
||
).ConfigureAwait(false);
|
||
}
|
||
|
||
}
|
||
|
||
/// <summary>
|
||
/// 切换飞行模式，返回的 <see cref="Task"/> 实例在确认切换成功或超时后完成。
|
||
/// </summary>
|
||
/// <param name="mode">飞行模式。</param>
|
||
/// <param name="millisecondsTimeout">超时时间，单位为毫秒。</param>
|
||
/// <returns>表示切换模式操作的 <see cref="Task{TResult}"/> 实例。若切换模式成功，异步操作的结果为 true；否则为 false。</returns>
|
||
#if PRIVATE
|
||
public
|
||
#else
|
||
internal
|
||
#endif
|
||
async Task<bool> SetModeAsync(FlightMode mode, int millisecondsTimeout = 5000)
|
||
{
|
||
if (_shouldFollow && mode != FlightMode.GUIDED) _shouldFollow = false;
|
||
// 王海, 20160317, 紧急悬停时可返航或降落。
|
||
if (mode == FlightMode.RTL || mode == FlightMode.LAND)
|
||
{
|
||
StopEmergencyHover();
|
||
}
|
||
if (!IsEmergencyHoverActive)
|
||
{
|
||
return await SetModeCoreAsync(mode, millisecondsTimeout).ConfigureAwait(false);
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 无论在不在紧急悬停状态下都可用的切换模式方法。
|
||
/// </summary>
|
||
/// <param name="mode">模式。</param>
|
||
/// <param name="millisecondsTimeout">超时。</param>
|
||
/// <returns>成功与否。</returns>
|
||
internal abstract Task<bool> SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000);
|
||
|
||
protected async Task EnsureChannelsSetAsync()
|
||
{
|
||
if (
|
||
(DesiredChannel1 != null && Channel1 != DesiredChannel1) ||
|
||
(DesiredChannel2 != null && Channel2 != DesiredChannel2) ||
|
||
(DesiredChannel3 != null && Channel3 != DesiredChannel3) ||
|
||
(DesiredChannel4 != null && Channel4 != DesiredChannel4) ||
|
||
(DesiredChannel5 != null && Channel5 != DesiredChannel5) ||
|
||
(DesiredChannel6 != null && Channel6 != DesiredChannel6) ||
|
||
(DesiredChannel7 != null && Channel7 != DesiredChannel7) ||
|
||
(DesiredChannel8 != null && Channel8 != DesiredChannel8)
|
||
) // Any channel not set.
|
||
{
|
||
await SetChannelsAsync().ConfigureAwait(false);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 不主动设置 State，仅发送飞往某点命令。
|
||
/// </summary>
|
||
/// <param name="lat">纬度。</param>
|
||
/// <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 void RaiseAltitudeChanged() => AltitudeChanged?.Invoke(this, EventArgs.Empty);
|
||
|
||
protected void RaiseAltitudeChangedIfNeeded()
|
||
{
|
||
if (_lastChangedAlt == null || Math.Abs(Altitude - _lastChangedAlt.Value) >= 0.5 || DateTime.Now.AddMilliseconds(-500) >= _lastRaiseAltitudeChangedTime)
|
||
{
|
||
RaiseAltitudeChanged();
|
||
_lastChangedAlt = Altitude;
|
||
_lastRaiseAltitudeChangedTime = DateTime.Now;
|
||
}
|
||
}
|
||
|
||
protected void RaiseAttitudeChanged() => AttitudeChanged?.Invoke(this, EventArgs.Empty);
|
||
|
||
protected void RaiseDataStreamReceived(PDataStreamType dataStreamType)
|
||
{
|
||
#if PRIVATE
|
||
DataStreamReceived?.Invoke(this, new DataStreamReceivedEventArgs((DataStreamType)dataStreamType));
|
||
#endif
|
||
}
|
||
|
||
protected void RaiseHeartbeatReceived(HeartbeatReceivedEventArgs e)
|
||
{
|
||
HeartbeatReceived?.Invoke(this, e);
|
||
}
|
||
|
||
protected void RaiseLocationChanged() => LocationChanged?.Invoke(this, EventArgs.Empty);
|
||
|
||
public void RefreashLoc()
|
||
{
|
||
RaiseLocationChangedIfNeeded(true);
|
||
}
|
||
|
||
protected void RaiseLocationChangedIfNeeded(bool forcemk=false)
|
||
{
|
||
//强制刷新位置
|
||
if (forcemk)
|
||
{
|
||
RaiseLocationChanged();
|
||
return;
|
||
}
|
||
//减少计算量，在模拟飞机很多时花时间
|
||
bool EnRaise = true;
|
||
if (_lastChangedLocation != null)
|
||
EnRaise = (Altitude != _lastChangedLocation.Altitude) ||
|
||
(Latitude != _lastChangedLocation.Latitude) ||
|
||
(Longitude != _lastChangedLocation.Longitude);
|
||
//if (_lastChangedLocation == null || this.CalcDistance(_lastChangedLocation) >= 0.5 || DateTime.Now.AddMilliseconds(-500) >= _lastRaiseLocationChangedTime)
|
||
if (EnRaise)
|
||
{
|
||
RaiseLocationChanged();
|
||
_lastChangedLocation = new PLLocation(this);
|
||
_lastRaiseLocationChangedTime = DateTime.Now;
|
||
}
|
||
|
||
}
|
||
|
||
protected void RaiseMissionItemReceived(MissionItemReceivedEventArgs e)
|
||
{
|
||
MissionItemReceived?.Invoke(this, e);
|
||
}
|
||
|
||
protected void RaisePairingCompleted(PairingCompletedEventArgs e)
|
||
{
|
||
var handler = PairingCompleted;
|
||
if (handler == null) return;
|
||
if (SynchronizationContext.Current == _uiSyncContext)
|
||
{
|
||
handler(this, e);
|
||
}
|
||
else
|
||
{
|
||
_uiSyncContext.Post(() => handler(this, e));
|
||
}
|
||
}
|
||
|
||
protected void RaiseSensorDataReceived(EventArgs e)
|
||
{
|
||
SensorDataReceived?.Invoke(this, e);
|
||
}
|
||
|
||
protected void RaiseSysStatusReceived(SystemStatusReceivedEventArgs e)
|
||
{
|
||
SystemStatusReceived?.Invoke(this, e);
|
||
}
|
||
|
||
|
||
protected void RaiseTextReceived(MessageCreatedEventArgs e)
|
||
{
|
||
TextReceived?.Invoke(this, e);
|
||
}
|
||
|
||
|
||
|
||
protected void SetTargets(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null)
|
||
{
|
||
if (ch1 != null) DesiredChannel1 = ch1;
|
||
if (ch2 != null) DesiredChannel2 = ch2;
|
||
if (ch3 != null) DesiredChannel3 = ch3;
|
||
if (ch4 != null) DesiredChannel4 = ch4;
|
||
if (ch5 != null) DesiredChannel5 = ch5;
|
||
if (ch6 != null) DesiredChannel6 = ch6;
|
||
if (ch7 != null) DesiredChannel7 = ch7;
|
||
if (ch8 != null) DesiredChannel8 = ch8;
|
||
if (yaw != null) DesiredYaw = yaw;
|
||
}
|
||
|
||
protected virtual void UpdateFlightDataIfNeeded()
|
||
{
|
||
if (!TakeOffPoint.IsNullOrEmpty())
|
||
{
|
||
FlightDistance = TakeOffPoint.CalcDistance(this);
|
||
}
|
||
}
|
||
|
||
protected void UpdateFlightTimeSpanIfNeeded()
|
||
{
|
||
if (_takeOffTime != null && IsUnlocked)
|
||
{
|
||
var flightTimeSpan = DateTime.Now - _takeOffTime.Value;
|
||
if (flightTimeSpan - FlightTimeSpan >= _intervalToUpdateFlightTimeSpan)
|
||
{
|
||
FlightTimeSpan = flightTimeSpan;
|
||
}
|
||
}
|
||
}
|
||
|
||
private void CopterImplSharedPart_PropertyChanged(object sender, PropertyChangedEventArgs e)
|
||
{
|
||
switch (e.PropertyName)
|
||
{
|
||
case nameof(IsConnected):
|
||
if (!IsConnected)
|
||
{
|
||
IsCheckingConnection = false;
|
||
IsAbsolutelyConnected = false;
|
||
}
|
||
break;
|
||
|
||
case nameof(IsUnlocked):
|
||
if (IsUnlocked)
|
||
{
|
||
// 王海, 20151029, 在解锁时重置起飞点、起飞时间、飞行距离、飞行时间、速度。
|
||
TakeOffPoint = new PLLocation { Latitude = Latitude, Longitude = Longitude, Altitude = Altitude };
|
||
_takeOffTime = DateTime.Now;
|
||
FlightDistance = 0;
|
||
FlightDistance2D = 0;
|
||
FlightTimeSpan = TimeSpan.FromSeconds(0);
|
||
AirSpeed = 0;
|
||
GroundSpeed = 0;
|
||
}
|
||
else
|
||
{
|
||
if (IsEmergencyHoverActive) StopEmergencyHover();
|
||
}
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
private void SetFieldAndRaisePropertyChanged(ref ushort? field, ushort value, string propertyName)
|
||
{
|
||
if (field != value)
|
||
{
|
||
field = value;
|
||
RaisePropertyChanged(propertyName);
|
||
}
|
||
}
|
||
|
||
private void SetFieldAndRaisePropertyChanged(ref float? field, float value, string propertyName)
|
||
{
|
||
if (field != value)
|
||
{
|
||
field = value;
|
||
RaisePropertyChanged(propertyName);
|
||
}
|
||
}
|
||
|
||
private void SetStateAccordingToMode(FlightMode value, CopterState? defaultState = null)
|
||
{
|
||
switch (value)
|
||
{
|
||
case FlightMode.ALT_HOLD:
|
||
State = CopterState.FloatMode;
|
||
break;
|
||
|
||
case FlightMode.AUTO:
|
||
State = CopterState.TakingOff;
|
||
break;
|
||
|
||
case FlightMode.CIRCLE:
|
||
State = CopterState.Circling;
|
||
break;
|
||
|
||
case FlightMode.GUIDED:
|
||
State = CopterState.CommandMode;
|
||
break;
|
||
|
||
case FlightMode.LOITER:
|
||
State = CopterState.HoverMode;
|
||
break;
|
||
|
||
case FlightMode.RTL:
|
||
State = CopterState.Returning;
|
||
break;
|
||
|
||
case FlightMode.LAND:
|
||
State = CopterState.Landing;
|
||
break;
|
||
|
||
default:
|
||
if (defaultState != null) State = defaultState.Value;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|