[fix] 退出异常调整测试灯光

详细描述

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动

PlaneGcsSdk_Shared/Communication/TcpConnection.cs

@@ -80,7 +80,8 @@ namespace Plane.Communication
                     var connectTask = _client.ConnectAsync(_remoteHostname, _remotePort);
                     if (await Task.WhenAny(connectTask, Task.Delay(5000)) == connectTask)
                     {
-                        await connectTask.ConfigureAwait(false);
+                        if (_client.Client != null)  //需要测试
+                            await connectTask.ConfigureAwait(false);
                     }
                     else
                     {
@@ -91,15 +92,27 @@ namespace Plane.Communication
                 catch (SocketException e)
                 {
                     logstr = e.Message;
+                    CloseClient(); // 关闭并清理客户端
                 }
                 catch (ObjectDisposedException)
                 {
+                    CloseClient(); // 关闭并清理客户端
+                }
+                catch (Exception)
+                {
+                    CloseClient(); // 处理其他可能的异常
+                   // throw; // 可选：重新抛出异常，以便调用者知道发生了错误
                 }
                 _isBroken = false;
             }
-            _stream = _client.GetStream();
+            if (_client != null)
+                _stream = _client.GetStream();
+        }
+        private void CloseClient()
+        {
+            _client?.Close(); // 如果 _client 不为 null，则关闭连接
+            _client = null; // 将 _client 设置为 null，以便垃圾回收器可以回收它
         }
-
 
         private void CreateClientAndConnect()
         {

PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs

@@ -46,7 +46,7 @@ namespace Plane.Communication
               //      bool bret ;
              //       bret = _client.Client != null;
                   //  bret = bret && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
-                    return _client.Client != null && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
+                    return _client != null && _client.Client != null && !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected);
                 }
                 catch (ObjectDisposedException)
                 {

PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs

@@ -439,72 +439,17 @@ namespace Plane.CommunicationManagement
         //测试用 灯光间隔1S闪烁
         public async Task TestLED(short id, string colorString)
         {
-            
-            MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
-            led.target_system = 1;
-            led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
-            led.instance = 0;
-            led.pattern = 0;
-            led.custom_len = 4;   //命令类型——测试灯光
-            led.custom_bytes = new byte[24];
-      
-            if (colorString == "") colorString = "330033";
-
-            Color color = (Color)ColorConverter.ConvertFromString("#" + colorString);
-
-            led.custom_bytes[0] = color.R;
-            led.custom_bytes[1] = color.G;
-            led.custom_bytes[2] = color.B;
-            led.custom_bytes[3] = 3;
-            byte[] data;           
-            data = MavlinkUtil.StructureToByteArray(led);
-
-            byte[] packet = new byte[data.Length + 6 + 2];
-            packet[0] = MAVLink.MAVLINK_STX;
-
-            packet[1] = (byte)(data.Length);
-            packet[2] = 1;
-            packet[3] = 255; // this is always 255 - MYGCS
-
-            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
-
-            packet[5] = MAVLink.MAVLINK_MSG_ID_LED_CONTROL;
-            int i = 6;
-            foreach (byte b in data)
-            {
-                packet[i] = b;
-                i++;
-            }
-           
-            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);          
-            checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[MAVLink.MAVLINK_MSG_ID_LED_CONTROL], checksum);
-
-            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
-            byte ck_b = (byte)(checksum >> 8); ///< Low byte
-
-            packet[i] = ck_a;
-            i += 1;
-            packet[i] = ck_b;
-            i += 1;
-
-
-            //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
-            byte[] senddata = packet;
-
-//             for (int times = 0; times < 14; times++)
-//             {
-//                 senddata = senddata.Concat(packet).ToArray();
-//             }
-
-//             byte[] myByteArray = Enumerable.Repeat((byte)0x08, 15).ToArray();
-// 
-//             senddata = senddata.Concat(myByteArray).ToArray();
-
+            byte[] packet = DoLEDCommandAsync(51, 0, 51);
             temp = !temp;
             while (temp)
             {
-                Message.Show("测试灯光，长度 = " + senddata.Length);
-                await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, senddata);
+                Message.Show("发送测试灯光");
+                await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+                //如果是广播同时发送到广播端口
+                if ((id==0)&& Recomisopen)
+                {
+                   await BroadcastSendAsync(packet);
+                }
                 await Task.Delay(1000).ConfigureAwait(false);
             } 
         }

PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs

@@ -22,7 +22,21 @@ namespace Plane.CommunicationManagement
         private bool starttime = false;
         private bool rtcm_run = false;
         private SerialPort RecomPort;
-        private bool Recomisopen = false;
+      //  public bool Recomisopen = false;
+        private bool _Recomisopen = false;
+        public bool Recomisopen
+        {
+            get { return _Recomisopen; }
+            set
+            {
+                if (_Recomisopen != value)
+                {
+                    _Recomisopen = value;
+                    //_RtcmInfoViewModel.SetRTKStatestr();
+                }
+           }
+        }
+
 
 
         private IEnumerable<ICopter> _allcopters;
@@ -191,7 +205,7 @@ namespace Plane.CommunicationManagement
             return GeneratePacket(MAVLink.MAVLINK_MSG_ID_PARAM_REQUEST_READ, req);
         }
 
-        private byte[] DoLEDCommandAsync()
+        private byte[] DoLEDCommandAsync(byte red,byte green,byte blue)
         {
             MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
             led.target_system = 1;
@@ -200,11 +214,10 @@ namespace Plane.CommunicationManagement
             led.pattern = 0;
             led.custom_len = 4;
             led.custom_bytes = new byte[24];
-            led.custom_bytes[0] = 255;
-            led.custom_bytes[1] = 255;
-            led.custom_bytes[2] = 255;
-            led.custom_bytes[3] = 3;
-
+            led.custom_bytes[0] = red;
+            led.custom_bytes[1] = green;
+            led.custom_bytes[2] = blue;
+            led.custom_bytes[3] = 3;  //持续亮的时间=1/3hz=0.33s
             return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
         }
         private byte[] DoThrowoutCommandAsync()
@@ -487,8 +500,14 @@ namespace Plane.CommunicationManagement
             short copterId = 0;
             byte[] batchPacket = null;
             //部分开始任务
-            if (copters!=null)
-               GetCopterIds(copters, out copterId, out batchPacket);
+            if (copters != null)
+                GetCopterIds(copters, out copterId, out batchPacket);
+            else
+            {
+                //只有全部飞机才广播
+                if (Recomisopen)
+                    await BroadcastSendAsync(packet);
+            }
             await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
         }
 
@@ -502,7 +521,10 @@ namespace Plane.CommunicationManagement
             {
                 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
+                if (Recomisopen)
+                    await BroadcastSendAsync(packet);
+            }
+            else
                 Windows.Messages.Message.Show($"未开发完成！！");
         }
 
@@ -515,7 +537,10 @@ namespace Plane.CommunicationManagement
             {
                 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
+                if (Recomisopen)
+                    await BroadcastSendAsync(packet);
+            }
+            else
                 Windows.Messages.Message.Show($"未开发完成！！");
         }
 
@@ -540,6 +565,9 @@ namespace Plane.CommunicationManagement
 
                 byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray();
                 await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
+                if (Recomisopen)
+                    await BroadcastSendAsync(data);
+
             }
             else
             {
@@ -551,6 +579,13 @@ namespace Plane.CommunicationManagement
                         await vcopter.UnlockAsync();
                 }
             }
+            //发送到广播模块--只针对全部飞机
+            //if (enrecom&&(copters == null))
+            {
+               // if (enrecom)
+
+
+            }
         }
 
 
@@ -601,8 +636,10 @@ namespace Plane.CommunicationManagement
                     byte[] batchPacket = null;
                     GetCopterIds(copters, out copterId, out batchPacket);
 
-                    byte[] packet = DoLEDCommandAsync();
+                    byte[] packet = DoLEDCommandAsync(255,255,255);
                     await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+                    if (Recomisopen&&(copters == null))
+                        await BroadcastSendAsync(packet);
                 });
             }
             else
@@ -661,6 +698,8 @@ namespace Plane.CommunicationManagement
                 byte[] packet = SetParam2Async(paramname, value);
                 int packetNum = packet[2];
                 await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+                if (Recomisopen && (copters == null))
+                    await BroadcastSendAsync(packet);
                 return packetNum;
             }
             else
@@ -858,6 +897,8 @@ namespace Plane.CommunicationManagement
                 short copterId = 0;
                 byte[] batchPacket = null;
                 await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+                if (Recomisopen)
+                    await BroadcastSendAsync(packet);
             }
             else
             {
@@ -906,6 +947,8 @@ namespace Plane.CommunicationManagement
 
                 byte[] packet = SetModeAsync(FlightMode.LAND);
                 await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
+                if (Recomisopen&&(copters == null))
+                    await BroadcastSendAsync(packet);
             }
             else
             {
@@ -940,6 +983,8 @@ namespace Plane.CommunicationManagement
 
                 byte[] data = packet1.Concat(packet2).ToArray();
                 await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
+                if (Recomisopen&&(copters == null))
+                    await BroadcastSendAsync(data);
             }
             else
             {
@@ -1206,110 +1251,68 @@ namespace Plane.CommunicationManagement
         byte rtcm_tmpser = 0;  //用于通讯模块的
         byte rtcm_Broadser = 0;//用于广播的
 
-
-
-        public void BroadcastGpsDataAsync(byte[] data, ushort length)
+        /// <summary>
+        /// 发送到广播端口
+        /// </summary>
+        /// <param name="packet">发送数据</param>
+        /// <param name="reopensend">发送失败是否要重新打开再发一次</param>
+        /// <returns></returns>
+        public async Task BroadcastSendAsync(byte[] packet,bool reopensend = false)
         {
-            //广播发送RTCM数据采用专用数据可以一次发180个字节
-            if (!Recomisopen)
-                return;
-            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++)
+            
+            bool sendok = false;
+            //并且没有最后打开的端口说明已经人为关闭了
+            if (last_reserialport == "")
             {
-                datalen = Math.Min(length - a * msglen, msglen);
-                gps.data = new byte[msglen];
-                Array.Copy(data, a * msglen, gps.data, 0, datalen);
-                //gps.data[0] = rtcm_Broadser++;
-                gps.len = (byte)datalen;
-                gps.target_component = rtcm_Broadser++;
-                //实测一旦收到数据都是正确的，张伟已经做过crc检验了，为了兼容性不再做了
-                //如果需要做，飞控里面需要把注释的代码放开，并且不再兼容之前的固件
-                gps.target_system = 1;// checkrtrcmsum(gps.data, (ushort)datalen);  
-
-                //   Console.WriteLine("                                              ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen );
-
-                byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
+                return;
+            }
+            try
+            {
+                //防止阻塞，异步发送
+                await RecomPort.BaseStream.WriteAsync(packet, 0, packet.Length);
+                //RecomPort.Write(packet, 0, packet.Length);
+                sendok = true;
+            }
+            catch (Exception ex)
+            {
+                Windows.Messages.Message.Show("广播端口发送失败...");
+            }
 
+            if (!sendok&& reopensend)
+            {
+                //再次打开串口
                 try
                 {
-                    RecomPort.Write(packet, 0, packet.Length);
+                    ReOpenRtcmserial();
+                    if (Recomisopen) Windows.Messages.Message.Show("广播端口打开成功!");
                 }
                 catch (Exception ex)
                 {
-                    Windows.Messages.Message.Show("转发端口发送失败" + ex.Message);
+                    Windows.Messages.Message.Show("再次打开串口失败" + ex.Message);
+                    return;
                 }
-
-               // await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
-
-            }
-
-
-
-
-            /*
-            MAVLink.mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t();
-            var msglen = 180;
-
-            if (length > msglen * 4)
-            {
-                Windows.Messages.Message.Show($"RTCM数据太长:" + length);
-                return;
-            }
-
-            // number of packets we need, including a termination packet if needed
-            var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1;
-
-            if (nopackets >= 4)
-                nopackets = 4;
-
-            // flags = isfrag(1)/frag(2)/seq(5)
-
-            for (int a = 0; a < nopackets; a++)
-            {
-                // check if its a fragment
-                if (nopackets > 1)
-                    gps.flags = 1;
-                else
-                    gps.flags = 0;
-
-                // add fragment number
-                gps.flags += (byte)((a & 0x3) << 1);
-
-                // add seq number
-                gps.flags += (byte)((inject_seq_no & 0x1f) << 3);
-
-                // create the empty buffer
-                gps.data = new byte[msglen];
-
-                // calc how much data we are copying
-                int copy = Math.Min(length - a * msglen, msglen);
-
-                // copy the data
-                Array.Copy(data, a * msglen, gps.data, 0, copy);
-
-                // set the length
-                gps.len = (byte)copy;
-                byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_RTCM_DATA, gps, gps.len);
-
-                //Console.WriteLine(string.Format("{0:T}  rtcm send :{1:D}", DateTime.Now, packet.Length));
-
-                try
+                //再次发送
+                if (Recomisopen)
                 {
-                        RecomPort.Write(packet, 0, packet.Length);
-                }
-                catch (Exception ex)
-                {
-                    Windows.Messages.Message.Show("转发端口发送失败" + ex.Message);
+                    try
+                    {
+                        await RecomPort.BaseStream.WriteAsync(packet, 0, packet.Length);
+                        //RecomPort.Write(packet, 0, packet.Length);
+                    }
+                    catch (Exception ex)
+                    {
+                        Windows.Messages.Message.Show("再次发送失败...");
+                    }
                 }
+
             }
-            inject_seq_no++;
-            */
         }
 
-
+        public async Task BroadcastbackupGpsDataAsync(byte[] packet)
+        {
+            await BroadcastSendAsync(packet,true);
+        }
+ 
         // get sum crc 计算数据校验和
         public byte checkrtrcmsum(byte[] data, ushort length)
         {
@@ -1328,7 +1331,7 @@ namespace Plane.CommunicationManagement
         /// <param name="data"></param>
         /// <param name="length"></param>
         /// <returns></returns>
-        public async Task InjectGpsDataAsync(byte[] data, ushort length)
+        public async Task InjectGpsDataAsync(byte[] data, ushort length,bool enrecom)
         {
             //由于通讯模块限制一次只能发110个字节
             if (UseTransModule)
@@ -1353,22 +1356,21 @@ namespace Plane.CommunicationManagement
 
                     byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
 
+                    //发送到通讯模块
                     await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
-                  
+                    //发送到广播端口作为备用数据源
+                    if (enrecom)
+                        await BroadcastbackupGpsDataAsync(packet);
                     await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
 
-                   // 重发一次，有序列号(target_component)飞机可以检测出来重复接收的
+                    // 重发一次，有序列号(target_component)飞机可以检测出来重复接收的
                     //需要新固件支持
                     await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
+                    //发送到广播端口作为备用数据源
+                    if (enrecom)
+                        await BroadcastbackupGpsDataAsync(packet);
+
                     await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
-
-                    
-                    //重发一次，有序列号(target_component)飞机可以检测出来重复接收的--需要新固件支持
-                    //await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
-                   // await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
-                    
-
-
                 }
             }
             else
@@ -1480,7 +1482,15 @@ namespace Plane.CommunicationManagement
             await Task.Delay(1);
         }
 
-        public void CloseResendRtcmserial()
+        public string last_reserialport ="";
+        public void ReOpenRtcmserial()
+        {
+            CloseResendRtcmserial();
+            OpenResendRtcmserial(last_reserialport);
+
+        }
+
+        public void CloseResendRtcmserial(bool cleanport=false)
         {
             if (Recomisopen)
             {
@@ -1488,21 +1498,41 @@ namespace Plane.CommunicationManagement
                 RecomPort.Dispose();
             }
             Recomisopen = false;
+            //清除端口
+            if (cleanport)
+                last_reserialport="";
+
         }
+
+        public string BoardPortStatusStr
+        {
+            get { 
+                if (Recomisopen)
+                    return "广播端口已打开"; 
+                else
+                    return "广播端口未打开";
+            }
+            
+        }
+
+
+
         public bool OpenResendRtcmserial(string reserialport)
         {
-           
-            RecomPort= new SerialPort(reserialport);
+            RecomPort = new SerialPort(reserialport);
             RecomPort.BaudRate = 57600;
             RecomPort.Parity = Parity.None;
             RecomPort.StopBits = StopBits.One;
             RecomPort.DataBits = 8;
             RecomPort.Handshake = Handshake.None;
-          
+            RecomPort.WriteTimeout = 500; // 设置写入超时为500毫秒,防止Close时候卡死
+
+
             try
             {
                 RecomPort.Open();
                 Recomisopen = true;
+                last_reserialport = reserialport;
             }
             catch (Exception ex)
             {