diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 30ea88f..3833b94 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -198,6 +198,23 @@ namespace Plane.CommunicationManagement return packetNum; } + + /// + /// 读取参数 + /// + /// + /// + /// + public async Task ReadParamAsnyc(string paramname, IEnumerable copters = null) + { + 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); + } + /// /// 起飞测试 /// @@ -235,6 +252,9 @@ namespace Plane.CommunicationManagement await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); } + + + /// /// 获取版本 /// @@ -338,13 +358,48 @@ namespace Plane.CommunicationManagement await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); } + /// + /// 校准加速计 + /// + /// + /// + public async Task DoStartPreflightCompassAsync(short copterId) + { + 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); + } + /// + /// 校准加速计下一步 + /// + /// + /// + public async Task DoNextPreflightCompassAsync(short copterId) + { + 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); + } + + /// + /// 校准指南针 + /// + /// + /// public async Task DoCalibrationCompassAsync(short copterId) { 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); } + /// + /// 放弃校准指南针 + /// + /// + /// public async Task DoCancelCalibrationCompassAsync(short copterId) { byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_CANCEL_MAG_CAL, 0, 0, 1, 0, 0, 0, 0); @@ -557,6 +612,22 @@ namespace Plane.CommunicationManagement return GeneratePacket((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req); } + private byte[] GetParam2Async(string paramname) + { + var paramId = Encoding.ASCII.GetBytes(paramname); + Array.Resize(ref paramId, 16); + + var req = new MAVLink.mavlink_param_request_read_t + { + param_index = -1, + target_system = 1, + target_component = 1, + param_id = paramId + }; + + return GeneratePacket(MAVLink.MAVLINK_MSG_ID_PARAM_REQUEST_READ, req); + } + private byte[] DoLEDCommandAsync() { MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control(); diff --git a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs index 7d5cab4..0c88891 100644 --- a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs +++ b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs @@ -614,10 +614,20 @@ namespace Plane.Copters protected set { Set(nameof(SatCount), ref _SatCount, value); } } + public float RetainInt + { + get { return BitConverter.ToSingle(Retain,0); } + } + + public byte[] Retain { get { return _Retain; } - protected set { Set(nameof(Retain), ref _Retain, value); } + protected set + { + Set(nameof(Retain), ref _Retain, value); + RaisePropertyChanged(nameof(RetainInt)); + } } public CopterState State diff --git a/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs b/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs index 5ba365e..c07b5cb 100644 --- a/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs +++ b/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs @@ -31,7 +31,7 @@ namespace Plane.Copters GpsHdop = _internalCopter.gpshdop; Altitude = _internalCopter.gpsalt; Retain = BitConverter.GetBytes(_internalCopter.retain); - Windows.Messages.Message.Show("_internalCopter.retain = " + _internalCopter.retain.ToString()); + Voltage = _internalCopter.battery_voltage; CommModuleMode = (FlightMode)_internalCopter.commModuleMode; CommModuleVersion = _internalCopter.commModuleVersion; diff --git a/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs b/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs index 1d275de..ea9390b 100644 --- a/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs @@ -58,7 +58,7 @@ namespace Plane.Copters public byte gpsstatus { get; set; } public float gpshdop { get; set; } public byte satcount { get; set; } - public int retain { get; set; } + public float retain { get; set; } public float groundspeed { get; set; } public float groundcourse { get; set; } public double lat { get; set; } diff --git a/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs index 92a91fc..1006063 100644 --- a/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs +++ b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs @@ -144,7 +144,7 @@ namespace Plane.Protocols public Int32 control_mode; public UInt32 lat; public UInt32 lng; - public Int32 retain; + public float retain; public Int32 alt; public Int16 gps_status;