diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs index cfb6856..2e04977 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs @@ -322,17 +322,26 @@ namespace Plane.CommunicationManagement //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet); byte[] senddata = packet; - for (int times = 0; times < 3; times++) + for (int times = 0; times < 4; times++) { senddata = senddata.Concat(packet).ToArray(); } + + temp = !temp; while (temp) { - await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet); - await Task.Delay(1000).ConfigureAwait(false); + await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, senddata); + await Task.Delay(50).ConfigureAwait(false); + await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, senddata); + await Task.Delay(50).ConfigureAwait(false); + await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, senddata); + await Task.Delay(50).ConfigureAwait(false); + await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, senddata); + await Task.Delay(50).ConfigureAwait(false); + await Task.Delay(800).ConfigureAwait(false); } } diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 286897d..5922b42 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -627,7 +627,9 @@ namespace Plane.CommunicationManagement // set the length gps.len = (byte)copy; + byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_GPS_RTCM_DATA, gps); + Console.WriteLine($"{DateTime.Now.ToLongTimeString()}---发送给张伟的数据长度 = {packet.Length}"); await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); } } diff --git a/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs b/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs index c07b5cb..2f727eb 100644 --- a/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs +++ b/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs @@ -36,7 +36,8 @@ namespace Plane.Copters CommModuleMode = (FlightMode)_internalCopter.commModuleMode; CommModuleVersion = _internalCopter.commModuleVersion; IsUnlocked = _internalCopter.isUnlocked; - Yaw = _internalCopter.yaw; + //Yaw = _internalCopter.yaw; + Heading = _internalCopter.heading; HeartbeatCount++; if (SatCount > 8) diff --git a/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs b/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs index 283d96e..93b93a6 100644 --- a/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs +++ b/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs @@ -390,8 +390,9 @@ namespace Plane.Copters isUnlocked = info.lock_status == 1; battery_voltage = ((float)info.battery_voltage) / 1000; - yaw = ((float)info.yaw / 100) % 360; + heading = (short)((info.heading / 100) % 360); + commModuleVersion = version; retain = info.retain; diff --git a/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs index 1006063..ea21ca8 100644 --- a/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs +++ b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs @@ -152,7 +152,7 @@ namespace Plane.Protocols public byte lock_status; public byte gps_num_sats; public Int16 battery_voltage; - public Int16 yaw; + public Int16 heading; };