diff --git a/PlaneGcsSdk.Contract_Shared/Copters/GpsFixType.cs b/PlaneGcsSdk.Contract_Shared/Copters/GpsFixType.cs index 77bd3aa..b2af0ea 100644 --- a/PlaneGcsSdk.Contract_Shared/Copters/GpsFixType.cs +++ b/PlaneGcsSdk.Contract_Shared/Copters/GpsFixType.cs @@ -6,7 +6,8 @@ Fix2D = 2, Fix3D = 3, DGPS3D=4, - RTK3D= 5 + RTKFLOAT= 5, + RTKFIXED = 6 } diff --git a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs b/PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs index b597f9e..d579501 100644 --- a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs +++ b/PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs @@ -112,8 +112,8 @@ namespace Plane.Copters Task SetShowLEDAsync(bool Ledon); Task SetShowLEDFlashAsync(int Flashtype, int millisecondsTime); - + Task SetShowRGBLEDAsync(int RedValue, int GreenValue, int BlueValue); /// /// 开始对频。 diff --git a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs index 51ee631..881bd32 100644 --- a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs +++ b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs @@ -886,6 +886,41 @@ namespace Plane.Copters 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( @@ -895,6 +930,7 @@ namespace Plane.Copters ch4: 1500 ).ConfigureAwait(false); } + } /// diff --git a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs b/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs index fcbdcf6..1f7c642 100644 --- a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs @@ -72,6 +72,13 @@ namespace Plane.Copters return TaskUtils.CompletedTask; } + public Task SetShowRGBLEDAsync(int RedValue, int GreenValue, int BlueValue) + { + return TaskUtils.CompletedTask; + } + + + public Task StartPairingAsync() { return TaskUtils.CompletedTask; diff --git a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs index 7c0b3f6..d693bbd 100644 --- a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs @@ -275,6 +275,14 @@ namespace Plane.Copters 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; diff --git a/PlaneGcsSdk_Shared/Copters/PLCopter.cs b/PlaneGcsSdk_Shared/Copters/PLCopter.cs index d2213c9..2794b65 100644 --- a/PlaneGcsSdk_Shared/Copters/PLCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/PLCopter.cs @@ -149,6 +149,17 @@ namespace Plane.Copters _showLed = Ledon; } + + + + public async Task SetShowRGBLEDAsync(int RedValue,int GreenValue,int BlueValue) + { + float rgbvalue = (RedValue & 255) << 16 + (GreenValue & 255) << 8 + (BlueValue & 255); + await SetParamNoCheckAsync("NTF_SHOWRGBLED", rgbvalue); + } + + + /// /// 闪烁SHOWLED,如果当前没亮,那就亮一下,如果本来就是亮的,那就暗一下 ///