diff --git a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs b/PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs index 3fdf0c9..43c88fc 100644 --- a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs +++ b/PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs @@ -131,8 +131,27 @@ namespace Plane.Copters /// 表示此命令异步发送操作的 实例。 Task StopPairingAsync(); - + /// + /// 发送命令 + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// Task DoCommandAsync(int actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7); + /// + /// 发送命令数据包已生成 + /// + /// + /// + /// + Task DoCommandAckAsync(ushort command, byte result); + } } diff --git a/PlaneGcsSdk_Shared/Communication/UdpServerConnectionManager.NET.cs b/PlaneGcsSdk_Shared/Communication/UdpServerConnectionManager.NET.cs index bd21cda..6148f86 100644 --- a/PlaneGcsSdk_Shared/Communication/UdpServerConnectionManager.NET.cs +++ b/PlaneGcsSdk_Shared/Communication/UdpServerConnectionManager.NET.cs @@ -41,6 +41,7 @@ namespace Plane.Communication { var connection = new UdpServerConnection(ep, _client.SendAsync); _connections.Add(remoteAddress, connection); + //调用AddOrUpdateCopter添加飞机 RaiseConnectionEstablished(connection, remoteAddress); } _connections[remoteAddress].EnqueueDatagram(data); diff --git a/PlaneGcsSdk_Shared/Communication/UdpServerConnectionManager.cs b/PlaneGcsSdk_Shared/Communication/UdpServerConnectionManager.cs index a6f4cef..a387e19 100644 --- a/PlaneGcsSdk_Shared/Communication/UdpServerConnectionManager.cs +++ b/PlaneGcsSdk_Shared/Communication/UdpServerConnectionManager.cs @@ -27,6 +27,11 @@ namespace Plane.Communication _connections.Clear(); } + public void DeleteConnections(string remoteAddress) + { + _connections.Remove(remoteAddress); + } + public void Dispose() { if (_disposed) diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index cd58d36..c8937d6 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -18,6 +18,7 @@ namespace Plane.CommunicationManagement private DateTime waitRtcmTime = DateTime.Now; private bool starttime = false; private bool rtcm_run = false; + private IEnumerable _allcopters; //是否使用专用传输模块 public bool UseTransModule = true; @@ -494,8 +495,13 @@ namespace Plane.CommunicationManagement } else { - foreach (var vcopter in copters) - await vcopter.UnlockAsync(); + IEnumerable vcopters = copters; + if (vcopters == null) vcopters = _allcopters; + if (vcopters != null) + { + foreach (var vcopter in vcopters) + await vcopter.UnlockAsync(); + } } } @@ -553,8 +559,13 @@ namespace Plane.CommunicationManagement } else { - foreach (var vcopter in copters) - await vcopter.SetParamAsync(paramname, value); + IEnumerable vcopters = copters; + if (vcopters == null) vcopters = _allcopters; + if (vcopters != null) + { + foreach (var vcopter in vcopters) + await vcopter.SetParamAsync(paramname, value); + } return 0; } } @@ -605,12 +616,24 @@ namespace Plane.CommunicationManagement } + public void SetAllCoptersForWifi(IEnumerable copters ) + { + + _allcopters = copters; + + } + + + /// /// 读取参数 /// /// /// /// + /// + + public async Task ReadParamAsnyc(string paramname, IEnumerable copters = null) { if (UseTransModule) @@ -730,8 +753,13 @@ namespace Plane.CommunicationManagement } else { - foreach (var vcopter in copters) - await vcopter.LandAsync(); + IEnumerable vcopters = copters; + if (vcopters == null) vcopters = _allcopters; + if (vcopters != null) + { + foreach (var vcopter in vcopters) + await vcopter.LandAsync(); + } } @@ -759,9 +787,13 @@ namespace Plane.CommunicationManagement } else { - foreach (var vcopter in copters) - await vcopter.LockAsync(); - + IEnumerable vcopters = copters; + if (vcopters == null) vcopters = _allcopters; + if (vcopters != null) + { + foreach (var vcopter in vcopters) + await vcopter.LockAsync(); + } } } @@ -790,13 +822,14 @@ namespace Plane.CommunicationManagement } else { - foreach (var vcopter in copters) - { - await vcopter.MotorTestAsync(1); - await vcopter.MotorTestAsync(2); - await vcopter.MotorTestAsync(3); - await vcopter.MotorTestAsync(4); - } + if (copters != null) + foreach (var vcopter in copters) + { + await vcopter.MotorTestAsync(1); + await vcopter.MotorTestAsync(2); + await vcopter.MotorTestAsync(3); + await vcopter.MotorTestAsync(4); + } } @@ -823,7 +856,8 @@ namespace Plane.CommunicationManagement } else { - foreach (var vcopter in copters) + if (copters != null) + foreach (var vcopter in copters) await vcopter.HoverAsync(); } @@ -848,6 +882,7 @@ namespace Plane.CommunicationManagement } else { + if (copters!=null) foreach (var vcopter in copters) await vcopter.FloatAsync(); @@ -872,8 +907,9 @@ namespace Plane.CommunicationManagement } else { - foreach (var vcopter in copters) - await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 0, 0, 0, 0, 1, 0, 0); + if (copters != null) + foreach (var vcopter in copters) + await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 0, 0, 0, 0, 1, 0, 0); } } @@ -885,23 +921,26 @@ namespace Plane.CommunicationManagement /// public async Task DoNextPreflightCompassAsync(IEnumerable copters = null) { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + MAVLink.mavlink_command_ack_t req = new MAVLink.mavlink_command_ack_t(); + req.command = 1; + req.result = 1; + if (UseTransModule) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); - - MAVLink.mavlink_command_ack_t req = new MAVLink.mavlink_command_ack_t(); - req.command = 1; - req.result = 1; - byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_ACK, req); await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); } else { - Windows.Messages.Message.Show($"未开发完成!!"); - + if (copters != null) + foreach (var vcopter in copters) + { + await vcopter.DoCommandAckAsync(1, 1); + } } } @@ -923,8 +962,9 @@ namespace Plane.CommunicationManagement } else { - foreach (var vcopter in copters) - await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.DO_START_MAG_CAL, 0, 1, 1, 0, 0, 0, 0); + if (copters != null) + foreach (var vcopter in copters) + await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.DO_START_MAG_CAL, 0, 1, 1, 0, 0, 0, 0); } } @@ -936,8 +976,15 @@ namespace Plane.CommunicationManagement /// public async Task DoCancelCalibrationCompassAsync(short copterId) { - byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_CANCEL_MAG_CAL, 0, 0, 1, 0, 0, 0, 0); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + if (UseTransModule) + { + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_CANCEL_MAG_CAL, 0, 0, 1, 0, 0, 0, 0); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + } + else + { + Windows.Messages.Message.Show($"未开发完成!!"); + } } /// /// 确认校准磁罗盘 @@ -961,20 +1008,28 @@ namespace Plane.CommunicationManagement /// public async Task InjectGpsDataAsync(byte[] data, ushort length) { - MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t(); - var msglen = 110; - int datalen = 0; - var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1; - for (int a = 0; a < len; a++) + if (UseTransModule) { - datalen = Math.Min(length - a * msglen, msglen); - gps.data = new byte[msglen]; - Array.Copy(data, a * msglen, gps.data, 0, datalen); - gps.len = (byte)datalen; - gps.target_component = 1; - gps.target_system = 1; - byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len); - await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t(); + var msglen = 110; + int datalen = 0; + var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1; + for (int a = 0; a < len; a++) + { + datalen = Math.Min(length - a * msglen, msglen); + gps.data = new byte[msglen]; + Array.Copy(data, a * msglen, gps.data, 0, datalen); + gps.len = (byte)datalen; + gps.target_component = 1; + gps.target_system = 1; + byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len); + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + } + }else + { + if (_allcopters != null) + foreach (var vcopter in _allcopters) + await vcopter.InjectGpsDataAsync(data, length); } } diff --git a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs b/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs index 3f29b1a..23f81d0 100644 --- a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs @@ -64,6 +64,10 @@ namespace Plane.Copters recnumber = 0; sendnumber = 0; } + public Task DoCommandAckAsync(ushort command, byte result) + { + return TaskUtils.CompletedTask; + } //重设数据量 diff --git a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs index ecdb6e0..f953a14 100644 --- a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs @@ -97,6 +97,10 @@ namespace Plane.Copters public FakeCopter() : this(SynchronizationContext.Current) { } + public Task DoCommandAckAsync(ushort command, byte result) + { + return TaskUtils.CompletedTask; + } /// /// 创建 实例。 diff --git a/PlaneGcsSdk_Shared/Copters/PLCopter.cs b/PlaneGcsSdk_Shared/Copters/PLCopter.cs index 471672e..7bd564b 100644 --- a/PlaneGcsSdk_Shared/Copters/PLCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/PLCopter.cs @@ -356,6 +356,14 @@ namespace Plane.Copters } } + public async Task DoCommandAckAsync(ushort command, byte result) + { + MAVLink.mavlink_command_ack_t req = new MAVLink.mavlink_command_ack_t(); + req.command = command; + req.result = result; + await _internalCopter.GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_ACK, req); + } + public async Task DoCommandAsync(int actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) { diff --git a/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs b/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs index ea9390b..51df102 100644 --- a/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs @@ -737,6 +737,8 @@ namespace Plane.Copters /// Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| return await DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, 0); } + + public async Task DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) { giveComport = true;