diff --git a/PlaneGcsSdk.Contract_Shared/Copters/FlightCommand.cs b/PlaneGcsSdk.Contract_Shared/Copters/FlightCommand.cs
index 52f1bf0..f6f5887 100644
--- a/PlaneGcsSdk.Contract_Shared/Copters/FlightCommand.cs
+++ b/PlaneGcsSdk.Contract_Shared/Copters/FlightCommand.cs
@@ -33,6 +33,11 @@
///
/// 命令飞行器起飞。
///
- TakeOff = 22
+ TakeOff = 22,
+
+ ///
+ /// 命令LED颜色改变
+ ///
+ LEDControl = 203
}
}
diff --git a/PlaneGcsSdk.Contract_Shared/Copters/IMission.cs b/PlaneGcsSdk.Contract_Shared/Copters/IMission.cs
index 2145fe9..a69001b 100644
--- a/PlaneGcsSdk.Contract_Shared/Copters/IMission.cs
+++ b/PlaneGcsSdk.Contract_Shared/Copters/IMission.cs
@@ -49,5 +49,20 @@
/// 获取或设置序号。
///
ushort Sequence { get; set; }
- }
+
+ ///
+ /// 获取或设置LED控制RGB的Red
+ ///
+ float R { get; set; }
+
+ ///
+ /// 获取或设置LED控制RGB的Green
+ ///
+ float G { get; set; }
+
+ ///
+ /// 获取或设置LED控制RGB的Blue
+ ///
+ float B { get; set; }
+}
}
diff --git a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
index 2da9e88..3a0e62b 100644
--- a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
+++ b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
@@ -157,6 +157,11 @@ namespace Plane.Copters
private float _Yaw;
+ //初始时位置:使用地图Center
+ private double _RecordLat;
+
+ private double _RecordLng;
+
#endregion Backing Fields
public CopterImplSharedPart(SynchronizationContext uiSyncContext) : base(uiSyncContext)
@@ -560,6 +565,18 @@ namespace Plane.Copters
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); }
+ }
+
///
/// 获取任务总数。
///
diff --git a/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs b/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs
index 9da7187..608f7f3 100644
--- a/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs
+++ b/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs
@@ -26,19 +26,22 @@ namespace Plane.Copters
{
case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
{
- Latitude = _internalCopter.lat;
- Longitude = _internalCopter.lng;
SatCount = _internalCopter.satcount;
GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
GpsHdop = _internalCopter.gpshdop;
Elevation = _internalCopter.gpsalt;
-
+
if (IsGpsAccurate)
{
- UpdateFlightDataIfNeeded();
- RaiseLocationChangedIfNeeded();
+ RecordLat = _internalCopter.lat;
+ RecordLng = _internalCopter.lng;
}
+ Latitude = RecordLat;
+ Longitude = RecordLng;
+ UpdateFlightDataIfNeeded();
+ RaiseLocationChangedIfNeeded();
+
RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
break;
diff --git a/PlaneGcsSdk_Shared/Copters/EHCopter.Mission.cs b/PlaneGcsSdk_Shared/Copters/EHCopter.Mission.cs
index 48607b0..e12c075 100644
--- a/PlaneGcsSdk_Shared/Copters/EHCopter.Mission.cs
+++ b/PlaneGcsSdk_Shared/Copters/EHCopter.Mission.cs
@@ -240,9 +240,19 @@ namespace Plane.Copters
req.autocontinue = 1;
req.frame = (byte)frame;
- req.x = (float)mission.Latitude;
- req.y = (float)mission.Longitude;
- req.z = mission.Altitude;
+ if (mission.Command == FlightCommand.LEDControl)
+ {
+ req.x = mission.R;
+ req.y = mission.G;
+ req.z = mission.B;
+ }
+ else
+ {
+ req.x = (float)mission.Latitude;
+ req.y = (float)mission.Longitude;
+ req.z = mission.Altitude;
+ }
+
req.param1 = mission.Param1;
req.param2 = mission.Param2;
diff --git a/PlaneGcsSdk_Shared/Copters/Mission.cs b/PlaneGcsSdk_Shared/Copters/Mission.cs
index 4c84ad0..c0d8d57 100644
--- a/PlaneGcsSdk_Shared/Copters/Mission.cs
+++ b/PlaneGcsSdk_Shared/Copters/Mission.cs
@@ -19,6 +19,9 @@ namespace Plane.Copters
private float _Param3;
private float _Param4;
private ushort _Sequence;
+ private float _R;
+ private float _G;
+ private float _B;
#endregion Backing Fields
@@ -107,6 +110,26 @@ namespace Plane.Copters
set { Set(nameof(Sequence), ref _Sequence, value); }
}
+ public float R
+ {
+ get { return _R; }
+ set { Set(nameof(R), ref _R, value); }
+ }
+
+ public float G
+ {
+ get { return _G; }
+ set { Set(nameof(G), ref _G, value); }
+ }
+
+ public float B
+ {
+ get { return _B; }
+ set { Set(nameof(B), ref _B, value); }
+ }
+
+
+
///
/// 创建降落任务。
///
@@ -160,7 +183,16 @@ namespace Plane.Copters
Altitude = alt
};
-
-
+ public static IMission CreateLEDControlMission(int delay, int ledmode, int ledrate, int ledtimes, byte red, byte green, byte blue) => new Mission
+ {
+ Command = FlightCommand.LEDControl,
+ Param1 = delay, //灯光延时 单位:100ms
+ Param2 = ledmode, //灯光模式 0.常亮 1.闪烁 2.随机闪烁(RGB无意义)
+ Param3 = ledrate, //闪烁延时 单位:100ms
+ Param4 = ledtimes, //次数 (暂无意义)
+ R = red, //Red
+ G = green, //Green
+ B = blue //Blue
+ };
}
}