From 772bafa7711406109f0f31168c8255d61041a2a3 Mon Sep 17 00:00:00 2001 From: xu Date: Mon, 2 Mar 2020 17:15:53 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E9=80=9A=E8=AE=AF=E4=B8=AD?= =?UTF-8?q?=EF=BC=8C=E5=8A=A0=E5=85=A5=E9=80=9A=E8=AE=AF=E6=A8=A1=E5=BC=8F?= =?UTF-8?q?=E5=8F=82=E6=95=B0=20UseTransModule?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CommModuleGenerateMavLink.cs | 291 +++++++++++------- 1 file changed, 185 insertions(+), 106 deletions(-) diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 27aab08..0ea4a46 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -1,5 +1,6 @@ using Plane.Copters; using Plane.Protocols; +using Plane.Windows.Messages; using System; using System.Collections.Generic; using System.Linq; @@ -17,8 +18,8 @@ namespace Plane.CommunicationManagement private DateTime waitRtcmTime = DateTime.Now; private bool starttime = false; private bool rtcm_run = false; - //是否单独处理传输 - private bool IsSingleTrans=false; + //是否使用专用传输模块 + private bool UseTransModule = true; private byte[] SetChannels(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null) { @@ -300,9 +301,9 @@ namespace Plane.CommunicationManagement public async Task SetChannelsAsync(IEnumerable copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null) { - if (IsSingleTrans) - await SetChannelsAsync_Single(copters, ch1, ch2, ch3, ch4); - else await SetChannelsAsync_CMod(copters, ch1, ch2, ch3, ch4); + if (UseTransModule) + await SetChannelsAsync_CMod(copters, ch1, ch2, ch3, ch4); + else await SetChannelsAsync_Single(copters, ch1, ch2, ch3, ch4); } public async Task SetChannelsAsync_Single(IEnumerable copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null) { @@ -330,9 +331,9 @@ namespace Plane.CommunicationManagement public async Task WriteMissionListAsync(ICopter copter, List missions) { - if (IsSingleTrans) - return await WriteMissionListAsync_Single(copter, missions); - else return await WriteMissionListAsync_CMod(short.Parse(copter.Id), missions); + if (UseTransModule) + return await WriteMissionListAsync_CMod(short.Parse(copter.Id), missions); + else return await WriteMissionListAsync_Single(copter, missions); } @@ -415,9 +416,9 @@ namespace Plane.CommunicationManagement /// public async Task DoMissionStartAsync(IEnumerable copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) { - if (IsSingleTrans) - await DoMissionStartAsync_Single(copters,hour_utc, minute_utc, second_utc, Missionlng, Missionlat); - else await DoMissionStartAsync_CMod(hour_utc, minute_utc, second_utc, Missionlng, Missionlat); + if (UseTransModule) + await DoMissionStartAsync_CMod(hour_utc, minute_utc, second_utc, Missionlng, Missionlat); + else await DoMissionStartAsync_Single(copters, hour_utc, minute_utc, second_utc, Missionlng, Missionlat); } public async Task DoMissionStartAsync_Single(IEnumerable copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) @@ -442,13 +443,18 @@ namespace Plane.CommunicationManagement await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); } + /// /// 暂停任务 /// public async Task DoMissionPauseAsync() { - byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, 0, 0, 0, 0, 0, 1, 0); - await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + if (UseTransModule) + { + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, 0, 0, 0, 0, 0, 1, 0); + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + }else + Windows.Messages.Message.Show($"未开发完成！！"); } /// @@ -456,8 +462,12 @@ namespace Plane.CommunicationManagement /// public async Task DoMissionResumeAsync(int hour_utc, int minute_utc, int second_utc) { - byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, 0, 0 , 2, 0); - await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + if (UseTransModule) + { + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, 0, 0, 2, 0); + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + }else + Windows.Messages.Message.Show($"未开发完成！！"); } /// @@ -467,18 +477,23 @@ namespace Plane.CommunicationManagement /// public async Task UnlockAsync(IEnumerable copters = null) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters,out copterId, out batchPacket); + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); - byte[] packet1 = SetChannels(1500, 1500, 1100, 1500); - - byte[] packet2 = SetModeAsync(FlightMode.ALT_HOLD); + byte[] packet1 = SetChannels(1500, 1500, 1100, 1500); - byte[] packet3 = DoARMAsync(true); + byte[] packet2 = SetModeAsync(FlightMode.ALT_HOLD); - byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray(); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + byte[] packet3 = DoARMAsync(true); + + byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray(); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + } + else + Windows.Messages.Message.Show($"未开发完成！！"); } /// @@ -488,15 +503,20 @@ namespace Plane.CommunicationManagement /// public void LED_FlickerAsync(IEnumerable copters = null) { - Task.Run(async () => + if (UseTransModule) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); + Task.Run(async () => + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); - byte[] packet = DoLEDCommandAsync(); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); - }); + byte[] packet = DoLEDCommandAsync(); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + }); + } + else + Windows.Messages.Message.Show($"未开发完成！！"); } @@ -509,14 +529,22 @@ namespace Plane.CommunicationManagement /// public async Task SetParamAsync(string paramname, float value, IEnumerable copters = null) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); - - byte[] packet = SetParam2Async(paramname, value); - int packetNum = packet[2]; - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); - return packetNum; + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = SetParam2Async(paramname, value); + int packetNum = packet[2]; + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + return packetNum; + } + else + { + Windows.Messages.Message.Show($"未开发完成！！"); + return 0; + } } /// @@ -526,21 +554,29 @@ namespace Plane.CommunicationManagement /// public async Task SetMultipleParamAsync(params string[] param) { - if (param.Length % 2 == 1) - return 0; - - byte[] packet = null; - int packetNum = 0; - for (int i = 0; i < param.Length; i += 2) + if (UseTransModule) { - string paramname = param[i]; - float value = float.Parse(param[i + 1]); - byte[] data = SetParam2Async(paramname, value); - packetNum = data[2]; - packet = packet == null ? data : packet.Concat(data).ToArray(); + if (param.Length % 2 == 1) + return 0; + + byte[] packet = null; + int packetNum = 0; + for (int i = 0; i < param.Length; i += 2) + { + string paramname = param[i]; + float value = float.Parse(param[i + 1]); + byte[] data = SetParam2Async(paramname, value); + packetNum = data[2]; + packet = packet == null ? data : packet.Concat(data).ToArray(); + } + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + return packetNum; + } + else + { + Windows.Messages.Message.Show($"未开发完成！！"); + return 0; } - await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - return packetNum; } @@ -552,12 +588,20 @@ namespace Plane.CommunicationManagement /// public async Task ReadParamAsnyc(string paramname, IEnumerable copters = null) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); - byte[] packet = GetParam2Async(paramname); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + byte[] packet = GetParam2Async(paramname); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + else + { + Windows.Messages.Message.Show($"未开发完成！！"); + + } } /// @@ -568,18 +612,23 @@ namespace Plane.CommunicationManagement /// public async Task TakeOffAsync(float alt, IEnumerable copters = null) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); - byte[] packet1 = SetModeAsync(FlightMode.GUIDED); + byte[] packet1 = SetModeAsync(FlightMode.GUIDED); - byte[] packet2 = SetChannels(1500, 1500, 1500, 1500); + byte[] packet2 = SetChannels(1500, 1500, 1500, 1500); - byte[] packet3 = DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, alt); + byte[] packet3 = DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, alt); - byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray(); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray(); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + } + else + Windows.Messages.Message.Show($"未开发完成！！"); } /// @@ -589,12 +638,17 @@ namespace Plane.CommunicationManagement /// public async Task ReturnToLaunchAsync(IEnumerable copters = null) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); - byte[] packet = SetModeAsync(FlightMode.RTL); - await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + byte[] packet = SetModeAsync(FlightMode.RTL); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + else + Windows.Messages.Message.Show($"未开发完成！！"); } @@ -607,14 +661,19 @@ namespace Plane.CommunicationManagement /// public async Task GetVersionsAsync(IEnumerable copters = null) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); - MAVLink.mavlink_sversion_request req = new MAVLink.mavlink_sversion_request(); - req.version = 0; - byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, req); - await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + MAVLink.mavlink_sversion_request req = new MAVLink.mavlink_sversion_request(); + req.version = 0; + byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, req); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + else + Windows.Messages.Message.Show($"未开发完成！！"); } /// /// 降落 @@ -622,12 +681,17 @@ namespace Plane.CommunicationManagement /// public async Task LandAsync(IEnumerable copters = null) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); - byte[] packet = SetModeAsync(FlightMode.LAND); - await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + byte[] packet = SetModeAsync(FlightMode.LAND); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + else + Windows.Messages.Message.Show($"未开发完成！！"); } /// @@ -637,16 +701,21 @@ namespace Plane.CommunicationManagement /// public async Task LockAsync(IEnumerable copters = null) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); - byte[] packet1 = SetChannels(1500, 1500, 1100, 1500); + byte[] packet1 = SetChannels(1500, 1500, 1100, 1500); - byte[] packet2 = DoARMAsync(false); + byte[] packet2 = DoARMAsync(false); - byte[] data = packet1.Concat(packet2).ToArray(); - await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + byte[] data = packet1.Concat(packet2).ToArray(); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + } + else + Windows.Messages.Message.Show($"未开发完成！！"); } /// @@ -656,18 +725,23 @@ namespace Plane.CommunicationManagement /// public async Task MotorTestAsync(IEnumerable copters = null) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); - //1-4代表1-4电机 - byte[] data1 = DoMotorTestAsync(1, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5); - byte[] data2 = DoMotorTestAsync(2, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5); - byte[] data3 = DoMotorTestAsync(3, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5); - byte[] data4 = DoMotorTestAsync(4, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5); + //1-4代表1-4电机 + byte[] data1 = DoMotorTestAsync(1, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5); + byte[] data2 = DoMotorTestAsync(2, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5); + byte[] data3 = DoMotorTestAsync(3, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5); + byte[] data4 = DoMotorTestAsync(4, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5); - byte[] data = data1.Concat(data2).Concat(data3).Concat(data4).ToArray(); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket); + byte[] data = data1.Concat(data2).Concat(data3).Concat(data4).ToArray(); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket); + } + else + Windows.Messages.Message.Show($"未开发完成！！"); } /// @@ -676,16 +750,21 @@ namespace Plane.CommunicationManagement /// public async Task HoverAsync(IEnumerable copters = null) { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); - byte[] packet1 = SetChannels(1500, 1500, 1500, 1500); + byte[] packet1 = SetChannels(1500, 1500, 1500, 1500); - byte[] packet2 = SetModeAsync(FlightMode.LOITER); + byte[] packet2 = SetModeAsync(FlightMode.LOITER); - byte[] data = packet1.Concat(packet2).ToArray(); - await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + byte[] data = packet1.Concat(packet2).ToArray(); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + } + else + Windows.Messages.Message.Show($"未开发完成！！"); } ///