From 4ae451aded1b47b3652083636a8f74d200f21825 Mon Sep 17 00:00:00 2001 From: zxd Date: Fri, 25 Jan 2019 11:58:04 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=94=AF=E6=8C=81=E5=A4=9A?= =?UTF-8?q?=E5=8F=B0=E9=A3=9E=E6=9C=BA=E6=A0=A1=E5=87=86=E9=80=9A=E8=AE=AF?= =?UTF-8?q?=E6=8E=A5=E5=8F=A3=E7=B1=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CommModuleGenerateMavLink.cs | 24 ++++++++++++++----- 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 3833b94..c5333c0 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -363,10 +363,14 @@ namespace Plane.CommunicationManagement /// /// /// - public async Task DoStartPreflightCompassAsync(short copterId) + public async Task DoStartPreflightCompassAsync(IEnumerable copters = null) { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 0, 0, 0, 0, 1, 0, 0); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); } /// @@ -374,14 +378,18 @@ namespace Plane.CommunicationManagement /// /// /// - public async Task DoNextPreflightCompassAsync(short copterId) + 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; byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_ACK, req); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); } /// @@ -389,10 +397,14 @@ namespace Plane.CommunicationManagement /// /// /// - public async Task DoCalibrationCompassAsync(short copterId) + public async Task DoCalibrationCompassAsync(IEnumerable copters = null) { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_START_MAG_CAL, 0, 1, 1, 0, 0, 0, 0); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); } ///