From 9c2238479f518ed0fe58945550345f5629660267 Mon Sep 17 00:00:00 2001 From: pxzleo Date: Wed, 17 Jan 2024 22:37:53 +0800 Subject: [PATCH] =?UTF-8?q?=E7=B4=A7=E6=80=A5=E8=BF=94=E8=88=AA=E5=8A=9F?= =?UTF-8?q?=E8=83=BD=EF=BC=8C=E7=AD=96=E7=95=A5=E4=B8=8D=E5=AF=B9=EF=BC=8C?= =?UTF-8?q?=E4=B8=B4=E6=97=B6=E4=BF=9D=E5=AD=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CommModuleGenerateMavLink.cs | 2921 +++++++++-------- PlaneGcsSdk_Shared/Copters/FakeCopter.cs | 126 +- PlaneGcsSdk_Shared/Copters/FlightMode.cs | 170 +- 3 files changed, 1675 insertions(+), 1542 deletions(-) diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 14311c6..68f0813 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -1,1445 +1,1476 @@ -using Plane.Copters; -using Plane.Protocols; -using System; -using System.Collections.Generic; -using System.IO.Ports; -using System.Linq; -using System.Text; -using System.Threading.Tasks; -using System.Windows.Media; -using static Plane.Copters.PlaneCopter; -using static Plane.Protocols.MAVLink; - -namespace Plane.CommunicationManagement -{ - public partial class CommModuleManager - { - public int packetcount = 0; - private static object lock_rtcm = new object();//互锁量 - private List rtcm_packets = new List(); - private DateTime waitRtcmTime = DateTime.Now; - private bool starttime = false; - private bool rtcm_run = false; - private SerialPort RecomPort; - private bool Recomisopen = false; - - - private IEnumerable _allcopters; - //是否使用专用传输模块 - public 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) - { - MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t(); - rc.target_component = 1; - rc.target_system = 1; - rc.chan1_raw = ch1 ?? 1500; - rc.chan2_raw = ch2 ?? 1500; - rc.chan3_raw = ch3 ?? 1500; - rc.chan4_raw = ch4 ?? 1500; - rc.chan5_raw = 1500; - rc.chan6_raw = 1500; - rc.chan7_raw = 1500; - rc.chan8_raw = 1500; - return GeneratePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc); - } - - private byte[] SetModeAsync(FlightMode mode) - { - return SetModeAsync(mode.ToAC2Mode()); - } - - private byte[] DoARMAsync(bool armit) - { - return DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0); - } - - private byte[] DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout) - { - return DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0); - } - - private byte[] DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) - { - MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t(); - - req.target_system = 1; - req.target_component = 1; - if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM) - { - req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL; - } - req.command = (ushort)actionid; - req.param1 = p1; - req.param2 = p2; - req.param3 = p3; - req.param4 = p4; - req.param5 = p5; - req.param6 = p6; - req.param7 = p7; - - return GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); ; - } - - private byte[] GeneratePacket(byte messageType, TMavlinkPacket indata) - { - byte[] data; - data = MavlinkUtil.StructureToByteArray(indata); - byte[] packet = new byte[data.Length + 6 + 2]; - packet[0] = MAVLink.MAVLINK_STX; - packet[1] = (byte)(data.Length); - packet[2] = (byte)packetcount; - packetcount++; - packet[3] = 255; // this is always 255 - MYGCS - packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; - packet[5] = messageType; - - 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[messageType], 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; - - return packet; - } - - - private byte[] SetModeAsync(ac2modes modein) - { - try - { - MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t(); - - if (TranslateMode(modein, ref mode)) - { - return SetModeAsync(mode); - } - } - catch { } - return null; - } - - private byte[] SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0) - { - mode.base_mode |= (byte)base_mode; - return GeneratePacket(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); - } - - private bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode) - { - mode.target_system = 1; - switch (modein) - { - case ac2modes.STABILIZE: - case ac2modes.AUTO: - case ac2modes.RTL: - case ac2modes.LOITER: - case ac2modes.ACRO: - case ac2modes.ALT_HOLD: - case ac2modes.CIRCLE: - case ac2modes.POSITION: - case ac2modes.LAND: - case ac2modes.OF_LOITER: - case ac2modes.GUIDED: - mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED; - mode.custom_mode = (uint)modein; - break; - default: - return false; - } - return true; - } - - private byte[] SetParam2Async(string paramname, float value) - { - // param type is set here, however it is always sent over the air as a float 100int = 100f. - var req = new MAVLink.mavlink_param_set_t { target_system = 1, target_component = 1, param_type = 4 }; - - byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname); - - Array.Resize(ref temp, 16); - req.param_id = temp; - req.param_value = (value); - - 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(); - 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]; - led.custom_bytes[0] = 255; - led.custom_bytes[1] = 255; - led.custom_bytes[2] = 255; - led.custom_bytes[3] = 3; - - return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led); - } - private byte[] DoThrowoutCommandAsync() - { - 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 = 1; //长度是1个字节 - led.custom_bytes = new byte[24]; - led.custom_bytes[0] = (byte)10; //10代表抛物 - return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led); - } - - - private byte[] DoTaskLEDCommandAsync(int ledmode, float ledInterval, int ledtimes, byte cR,byte cG,Byte cB) - { - //新版固件ledInterval间隔单位秒，传输为一个字节无法传输小数，所以*10改为100ms，可传输一位小数 - byte LEDInterval = (byte)(ledInterval * 10);// (byte)(10 / ledrate); - 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 = 6; //测试LED灯光 - led.custom_bytes = new byte[24]; - led.custom_bytes[0] = cR; - led.custom_bytes[1] = cG; - led.custom_bytes[2] = cB; - led.custom_bytes[3] =(byte)ledmode; - led.custom_bytes[4] = LEDInterval; - led.custom_bytes[5] = (byte)ledtimes; - return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led); - } - - - - private byte[] BatchCopterIdData(IEnumerable copters) - { - byte[] BatchByte = null; - List copterList = copters.OrderBy(c => int.Parse(c.Id)).ToList(); - for (int i = 0; i < copterList.Count; i++) - { - byte[] tempByte; - int id = int.Parse(copterList[i].Id); - int count = 1; - while (i + 1 < copterList.Count && int.Parse(copterList[i + 1].Id) == id + count) - { - count++; - i++; - } - - if (count == 1) - { - short curId = (short)(0 << 12 ^ id); - tempByte = BitConverter.GetBytes(curId); - } - else - { - short beginId = (short)(2 << 12 ^ id); - tempByte = BitConverter.GetBytes(beginId); - - short numCount = (short)(4 << 12 ^ count); - tempByte = tempByte.Concat(BitConverter.GetBytes(numCount)).ToArray(); - } - - if (BatchByte == null) - BatchByte = tempByte; - else - BatchByte = BatchByte.Concat(tempByte).ToArray(); - } - return BatchByte; - } - - private void GetCopterIds(IEnumerable copters, out short copterId, out byte[] batchPacket) - { - copterId = 0; - batchPacket = null; - - if (copters != null) - { - if (copters.Count() == 1) - copterId = short.Parse(copters.FirstOrDefault().Id); - else - { - batchPacket = BatchCopterIdData(copters); - } - } - } - - private async Task RtcmLoop() - { - while (rtcm_run) - { - byte[] sendDate = new byte[0]; - lock (lock_rtcm) - { - if (rtcm_packets.Count >= 3 || - (starttime && waitRtcmTime.AddMilliseconds(200) <= DateTime.Now)) - { - if (rtcm_packets.Count > 0) - { - Windows.Messages.Message.Show($"rtcm_packets.Count = {rtcm_packets.Count}"); - for (int i = 0; i < rtcm_packets.Count; i++) - { - sendDate = sendDate.Concat(rtcm_packets[i]).ToArray(); - } - rtcm_packets.Clear(); - starttime = false; - } - } - } - - if (sendDate != null && sendDate.Length > 0) - { - Windows.Messages.Message.Show($"{DateTime.Now.ToString("HH:mm:ss:fff")} 通信数据长度 = {sendDate.Length}"); - await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, sendDate).ConfigureAwait(false); - } - - await Task.Delay(10); - } - } - - /////////////////////////////////////////////////////////////公开输出函数 - /// - /// 设置通道 - /// - /// - /// - /// - /// - /// - /// - - - - public async Task SetChannelsAsync(IEnumerable copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null) - { - 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) - { - await Task.WhenAll(copters.Select(async copter => - await copter.SetChannelsAsync(ch2:ch2))).ConfigureAwait(false); - - } - public async Task SetChannelsAsync_CMod(IEnumerable copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null) - { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); - - byte[] packet = SetChannels(ch1, ch2, ch3, ch4); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); - } - - /// - /// 开始写任务 - /// - /// 飞机ID - /// 任务列表 - /// - /// - - public async Task WriteMissionListAsync(ICopter copter, List missions) - { - if (UseTransModule) - return await WriteMissionListAsync_CMod(short.Parse(copter.Id), missions); - else return await WriteMissionListAsync_Single(copter, missions); - - } - - public async Task WriteMissionListAsync_Single(ICopter copter, List missions) - { - return await copter.WriteMissionListAsync(missions); - } - - public async Task WriteMissionListAsync_CMod(short id, List missions) - { - missions.Insert(0, PLCopter.PRE_TAKE_OFF_MISSION); - List mission_list = new List(); - foreach (IMission mission in missions) - { - var frame = mission.Command == FlightCommand.Waypoint && mission.Sequence == 0 ? MAVLink.MAV_FRAME.GLOBAL : MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT; - - var req = new MAVLink.mavlink_mission_item_t(); - - req.target_system = 1; - req.target_component = 1; - - req.command = (byte)mission.Command; - - req.current = 0; - req.autocontinue = 1; - - req.frame = (byte)frame; - if (mission.Command == FlightCommand.LEDControl) - { - req.x = mission.R; - req.y = mission.G; - req.z = mission.B; - } - else - { - req.x = (float)mission.Latitude; - req.y = (float)mission.Longitude; - req.z = mission.Altitude; - } - - req.param1 = mission.Param1; - req.param2 = mission.Param2; - req.param3 = mission.Param3; - req.param4 = mission.Param4; - - req.seq = mission.Sequence; - mission_list.Add(req); - } - - bool result = await MissionPacket(id, (byte)MavComm.COMM_DOWNLOAD_MODE, mission_list.ToArray()); - - lock(MissinLocker) - { - if (missionWriteState.ContainsKey(id)) - { - missionWriteState[id].StateReturn = MissionErrorId; - missionWriteState[id].ErrorCode = ErrorCode; - missionWriteState[id].SendAchieved = result; - missionWriteState[id].WriteSucceed = false; - } - else - { - CommWriteMissinState state = new CommWriteMissinState(result); - state.StateReturn = MissionErrorId; - state.ErrorCode = ErrorCode; - missionWriteState.Add(id, state); - } - return result; - } - } - - /// - /// 开始执行任务 - /// - /// 开始执行的小时 - /// 开始执行的分钟 - /// 开始执行的秒 - /// 任务原点经度 - /// 任务原点纬度 - /// - public async Task DoMissionStartAsync(IEnumerable copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) - { - if (UseTransModule) - await DoMissionStartAsync_CMod(copters,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) - { - - foreach (var vcopter in copters) - { - await vcopter.MissionStartAsync(hour_utc, - minute_utc, - second_utc, - Missionlng, - Missionlat - ); - } - - } - - - public async Task DoMissionStartAsync_CMod(IEnumerable copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) - { - - byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, -1); - short copterId = 0; - byte[] batchPacket = null; - //部分开始任务 - if (copters!=null) - GetCopterIds(copters, out copterId, out batchPacket); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); - } - - - /// - /// 暂停任务 - /// - public async Task DoMissionPauseAsync() - { - 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($"未开发完成！！"); - } - - /// - /// 恢复任务 - /// - public async Task DoMissionResumeAsync(int hour_utc, int minute_utc, int second_utc) - { - 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($"未开发完成！！"); - } - - /// - /// 解锁 - /// - /// 要解锁的飞机 - /// - public async Task UnlockAsync(IEnumerable copters = null) - { - 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); //ALT_HOLD LOITER - - byte[] packet3 = DoARMAsync(true); - - byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray(); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); - } - else - { - IEnumerable vcopters = copters; - if (vcopters == null) vcopters = _allcopters; - if (vcopters != null) - { - foreach (var vcopter in vcopters) - await vcopter.UnlockAsync(); - } - } - } - - - /// - /// 抛物 - /// - /// 要解锁的飞机 - /// - public void ThrowoutAsync(IEnumerable copters = null) - { - if (UseTransModule) - { - Task.Run(async () => - { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); - - byte[] packet = DoThrowoutCommandAsync(); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); - }); - } - else - { - /* - Task.Run(async () => - { - foreach (var vcopter in copters) - await vcopter.LEDAsync(); - }); - */ - - } - } - - /// - /// LED闪烁白灯 - /// - /// 要闪烁的飞机 - /// - public void LED_FlickerAsync(IEnumerable copters = null) - { - if (UseTransModule) - { - 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); - }); - } - else - { - Task.Run(async () => - { - foreach (var vcopter in copters) - await vcopter.LEDAsync(); - }); - - } - - } - - /* - - private int ledmode = 0; //灯光模式 0常亮 1闪烁 2随机闪烁(RGB无意义) - private float ledInterval = 0; //间隔 单位秒 最小0.1 - private int ledtimes = 0; //次数 (预留、暂取消其意义) - private string ledRGB = "FFFFFF"; //灯光颜色 - - */ - - public void LED_TaskAsync( int ledmode, float ledInterval, int ledtimes, string ledRGB, ICopter copter,bool oldver=true ) - { - if (UseTransModule) - { - Task.Run(async () => - { - Color rightColor = (Color)ColorConverter.ConvertFromString("#" + ledRGB); - byte[] packet = DoTaskLEDCommandAsync(ledmode, ledInterval, ledtimes, (byte)rightColor.R, (byte)rightColor.G, (byte)rightColor.B); - await WriteCommPacketAsync(short.Parse(copter.Id), MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - }); - } - - - } - - - - /// - /// 设置参数 - /// - /// - /// - /// - /// - public async Task SetParamAsync(string paramname, float value, IEnumerable copters = null) - { - 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 - { - IEnumerable vcopters = copters; - if (vcopters == null) vcopters = _allcopters; - if (vcopters != null) - { - foreach (var vcopter in vcopters) - await vcopter.SetParamAsync(paramname, value); - } - return 0; - } - } - - /// - /// 广播设置多个参数 - /// - /// - /// - public async Task SetMultipleParamAsync(params string[] param) - { - if (param.Length % 2 == 1) - return 0; - if (UseTransModule) - { - - 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 - { - 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]); - // foreach (var vcopter in Copters ) - // await vcopter.SetParamAsync(paramname, value); - - } - - - - - return 0; - } - } - - - public void SetAllCoptersForWifi(IEnumerable copters ) - { - - _allcopters = copters; - - } - - - - /// - /// 读取参数 - /// - /// - /// - /// - /// - - - public async Task ReadParamAsnyc(string paramname, IEnumerable copters = null) - { - 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); - } - else - { - Windows.Messages.Message.Show($"未开发完成！！"); - - } - } - - /// - /// 起飞测试 - /// - /// 起飞高度 - /// - /// - public async Task TakeOffAsync(float alt, IEnumerable copters = null) - { - if (UseTransModule) - { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); - - byte[] packet1 = SetModeAsync(FlightMode.GUIDED); - - byte[] packet2 = SetChannels(1500, 1500, 1500, 1500); - - 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); - } - else - { - foreach (var vcopter in copters) - await vcopter.TakeOffAsync(); - - } - } - - /// - /// 返航 - /// - /// - /// - public async Task ReturnToLaunchAsync(IEnumerable copters = null) - { - 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); - } - else - { - foreach (var vcopter in copters) - await vcopter.ReturnToLaunchAsync (); - - } - - } - - - - - /// - /// 获取飞机版本 - /// - /// - /// - public async Task GetVersionsAsync(IEnumerable copters = null) - { - 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); - } - else - { - - Windows.Messages.Message.Show($"未开发完成！！"); - } - - } - - public async Task GetCommsumAsync(IEnumerable copters = null) - { - 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 = 1; - 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($"未开发完成！！"); - } - - } - - - - /// - /// 降落 - /// - /// - public async Task LandAsync(IEnumerable copters = null) - { - 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); - } - else - { - IEnumerable vcopters = copters; - if (vcopters == null) vcopters = _allcopters; - if (vcopters != null) - { - foreach (var vcopter in vcopters) - await vcopter.LandAsync(); - } - - } - - } - - /// - /// 加锁 - /// - /// - /// - public async Task LockAsync(IEnumerable copters = null) - { - if (UseTransModule) - { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); - - byte[] packet1 = SetChannels(1500, 1500, 1100, 1500); - - byte[] packet2 = DoARMAsync(false); - - byte[] data = packet1.Concat(packet2).ToArray(); - await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); - } - else - { - IEnumerable vcopters = copters; - if (vcopters == null) vcopters = _allcopters; - if (vcopters != null) - { - foreach (var vcopter in vcopters) - await vcopter.LockAsync(); - } - } - - } - - /// - /// 测试电机 - /// - /// - /// - public async Task MotorTestAsync(IEnumerable copters = null) - { - 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); - - byte[] data = data1.Concat(data2).Concat(data3).Concat(data4).ToArray(); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket); - } - else - { - if (copters != null) - foreach (var vcopter in copters) - { - await vcopter.MotorTestAsync(1); - await vcopter.MotorTestAsync(2); - await vcopter.MotorTestAsync(3); - await vcopter.MotorTestAsync(4); - } - - } - - } - - /// - /// 切换悬停模式 - /// - /// - public async Task HoverAsync(IEnumerable copters = null) - { - if (UseTransModule) - { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); - - byte[] packet1 = SetChannels(1500, 1500, 1500, 1500); - - byte[] packet2 = SetModeAsync(FlightMode.LOITER); - - byte[] data = packet1.Concat(packet2).ToArray(); - await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); - } - else - { - if (copters != null) - foreach (var vcopter in copters) - await vcopter.HoverAsync(); - - } - - } - - /// - /// 切换手动模式 - /// - /// - /// - public async Task FloatAsync(IEnumerable copters = null) - { - if (UseTransModule) - { - short copterId = 0; - byte[] batchPacket = null; - GetCopterIds(copters, out copterId, out batchPacket); - - byte[] packet = SetModeAsync(FlightMode.ALT_HOLD); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); - } - else - { - if (copters!=null) - foreach (var vcopter in copters) - await vcopter.FloatAsync(); - - } - } - - /// - /// 校准加速计 - /// - /// - /// - public async Task DoStartPreflightCompassAsync(IEnumerable copters = null) - { - if (UseTransModule) - { - 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, batchPacket).ConfigureAwait(false); - } - else - { - if (copters != null) - foreach (var vcopter in copters) - await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 0, 0, 0, 0, 1, 0, 0); - - } - } - - /// - /// 校准加速计下一步 - /// - /// - /// - 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; - - if (UseTransModule) - { - byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_ACK, req); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); - } - else - { - if (copters != null) - foreach (var vcopter in copters) - { - await vcopter.DoCommandAckAsync(1, 1); - } - } - } - - /// - /// 校准指南针 - /// - /// - /// - public async Task DoCalibrationCompassAsync(IEnumerable copters = null) - { - if (UseTransModule) - { - 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, batchPacket).ConfigureAwait(false); - } - else - { - if (copters != null) - foreach (var vcopter in copters) - await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.DO_START_MAG_CAL, 0, 1, 1, 0, 0, 0, 0); - - } - } - - /// - /// 放弃校准指南针 - /// - /// - /// - public async Task DoCancelCalibrationCompassAsync(short copterId) - { - if (UseTransModule) - { - byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_CANCEL_MAG_CAL, 0, 0, 1, 0, 0, 0, 0); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - } - else - { - Windows.Messages.Message.Show($"未开发完成！！"); - } - } - /// - /// 确认校准磁罗盘 - /// - /// - /// - public async Task DoAcceptCalibrationCompassAsync(short copterId) - { - byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_ACCEPT_MAG_CAL, 0, 0, 1, 0, 0, 0, 0); - await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - } - - - byte rtcm_tmpser = 0; - - - public void BroadcastGpsDataAsync(byte[] data, ushort length) - { - //广播发送RTCM数据采用专用数据可以一次发180个字节 - if (!Recomisopen) - return; - - 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 - { - RecomPort.Write(packet, 0, packet.Length); - } - catch (Exception ex) - { - Windows.Messages.Message.Show("转发端口发送失败" + ex.Message); - } - } - inject_seq_no++; - - } - - - - - /// - ///广播Rtk //-------------使用中的RTK广播函数------------ - /// - /// - /// - /// - public async Task InjectGpsDataAsync(byte[] data, ushort length) - { - //由于通讯模块限制一次只能发110个字节 - if (UseTransModule) - { - 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++) - { - 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_tmpser++; - gps.len = (byte)datalen; - gps.target_component = rtcm_tmpser++; - gps.target_system = 1; - - // Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen ); - - byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len); - - await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - - await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 - - //重发一次，有序列号(target_component)飞机可以检测出来重复接收的 - //需要新固件支持 - // await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - // await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 - - /* - //重发一次，有序列号(target_component)飞机可以检测出来重复接收的--需要新固件支持 - await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 - */ - - - } - } - else - { - if (_allcopters != null) - foreach (var vcopter in _allcopters) - await vcopter.InjectGpsDataAsync(data, length); - } - } - - int inject_seq_no = 0; - - /// - /// 插入RTK数据 - /// - /// - /// - /// - public async Task InjectGpsRTCMDataAsync(byte[] data, int length) - { - MAVLink.mavlink_gps_rtcm_data_t gps = new MAVLink.mavlink_gps_rtcm_data_t(); - var msglen = 180; //飞机端是180，之前也是180和通讯盒可能也有关系 - - // if (length > msglen * 4) - // log.Error("Message too large " + length); - - // number of packets we need, including a termination packet if needed - var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1; - - // if (nopackets>1) - // Plane.Windows.Messages.Message.Show(" RTCM_Mut: " + length); - - 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 = GeneratePacket(MAVLink.MAVLINK_MSG_ID_GPS_RTCM_DATA, gps); - - // packet[2] - //Plane.Windows.Messages.Message.Show("["+ packet[2]+"]RTCM:"+ gps.len); - - // Console.WriteLine("RTCM:" + gps.len+ "[" + data[0]+","+ data[1] + "," + data[2] + "," + data[3] + "," + data[4]+"]"); - - //Windows.Messages.Message.Show($"{DateTime.Now.ToString("HH:mm:ss:fff")} 单次长度 = {packet.Length}"); - //老版本在这发送，新版本在StartRtcmLoop里面一次性打包4个180字节发送了 - await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - /* //长RTCP数据发送还没开发完 - lock (lock_rtcm) - { - rtcm_packets.Add(packet); - if (rtcm_packets.Count == 1) - { - waitRtcmTime = DateTime.Now; - starttime = true; - } - } - */ - - } - } - - - - /// - /// 开始循环检测Rtcm数量 - /// - /// - public async Task StartRtcmLoop() - { - rtcm_run = true; - waitRtcmTime = DateTime.Now; - await Task.Factory.StartNew(RtcmLoop); - } - - /// - /// 结束循环检测Rtcm数量 - /// - /// - public async Task CloseRtcmLoop() - { - rtcm_run = false; - await Task.Delay(1); - } - - public void CloseResendRtcmserial() - { - if (Recomisopen) - { - RecomPort.Close(); - RecomPort.Dispose(); - } - Recomisopen = false; - } - public bool OpenResendRtcmserial(string reserialport) - { - - RecomPort= new SerialPort(reserialport); - RecomPort.BaudRate = 57600; - RecomPort.Parity = Parity.None; - RecomPort.StopBits = StopBits.One; - RecomPort.DataBits = 8; - RecomPort.Handshake = Handshake.None; - - try - { - RecomPort.Open(); - Recomisopen = true; - } - catch (Exception ex) - { - // Alert.Show("转发端口打开失败" + ex.Message); - } - - return Recomisopen; - } - - - - - /// - /// 产生RTK包 - /// - /// - /// - /// - /// - /// - public byte[] GenerateRTKPacketAsync(byte messageType, TMavlinkPacket indata, int rtklength) - { - byte[] data; - int rtkdataleng = rtklength + 3; - - data = MavlinkUtil.StructureToByteArray(indata); - - - byte[] packet = new byte[rtkdataleng + 6 + 2]; - - packet[0] = MAVLink.MAVLINK_STX; - packet[1] = (byte)(rtkdataleng); - packet[2] = (byte)packetcount; - packetcount++; - packet[3] = 255; // this is always 255 - MYGCS - packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; - packet[5] = messageType; - - int i = 6; - foreach (byte b in data) - { - if (i < rtkdataleng + 6) - packet[i] = b; - else break; - i++; - } - - ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6); - - checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], 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; - - return packet; - } - - - } - -} +using Plane.Copters; +using Plane.Geography; +using Plane.Protocols; +using System; +using System.Collections.Generic; +using System.IO.Ports; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Windows.Media; +using static Plane.Copters.PlaneCopter; +using static Plane.Protocols.MAVLink; + +namespace Plane.CommunicationManagement +{ + public partial class CommModuleManager + { + public int packetcount = 0; + private static object lock_rtcm = new object();//互锁量 + private List rtcm_packets = new List(); + private DateTime waitRtcmTime = DateTime.Now; + private bool starttime = false; + private bool rtcm_run = false; + private SerialPort RecomPort; + private bool Recomisopen = false; + + + private IEnumerable _allcopters; + //是否使用专用传输模块 + public 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) + { + MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t(); + rc.target_component = 1; + rc.target_system = 1; + rc.chan1_raw = ch1 ?? 1500; + rc.chan2_raw = ch2 ?? 1500; + rc.chan3_raw = ch3 ?? 1500; + rc.chan4_raw = ch4 ?? 1500; + rc.chan5_raw = 1500; + rc.chan6_raw = 1500; + rc.chan7_raw = 1500; + rc.chan8_raw = 1500; + return GeneratePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc); + } + + private byte[] SetModeAsync(FlightMode mode) + { + return SetModeAsync(mode.ToAC2Mode()); + } + + private byte[] DoARMAsync(bool armit) + { + return DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0); + } + + private byte[] DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout) + { + return DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0); + } + + private byte[] DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) + { + MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t(); + + req.target_system = 1; + req.target_component = 1; + if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM) + { + req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL; + } + req.command = (ushort)actionid; + req.param1 = p1; + req.param2 = p2; + req.param3 = p3; + req.param4 = p4; + req.param5 = p5; + req.param6 = p6; + req.param7 = p7; + + return GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); ; + } + + private byte[] GeneratePacket(byte messageType, TMavlinkPacket indata) + { + byte[] data; + data = MavlinkUtil.StructureToByteArray(indata); + byte[] packet = new byte[data.Length + 6 + 2]; + packet[0] = MAVLink.MAVLINK_STX; + packet[1] = (byte)(data.Length); + packet[2] = (byte)packetcount; + packetcount++; + packet[3] = 255; // this is always 255 - MYGCS + packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; + packet[5] = messageType; + + 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[messageType], 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; + + return packet; + } + + + private byte[] SetModeAsync(ac2modes modein) + { + try + { + MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t(); + + if (TranslateMode(modein, ref mode)) + { + return SetModeAsync(mode); + } + } + catch { } + return null; + } + + private byte[] SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0) + { + mode.base_mode |= (byte)base_mode; + return GeneratePacket(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); + } + + private bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode) + { + mode.target_system = 1; + switch (modein) + { + case ac2modes.STABILIZE: + case ac2modes.AUTO: + case ac2modes.RTL: + case ac2modes.LOITER: + case ac2modes.ACRO: + case ac2modes.ALT_HOLD: + case ac2modes.CIRCLE: + case ac2modes.POSITION: + case ac2modes.LAND: + case ac2modes.OF_LOITER: + case ac2modes.GUIDED: + mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED; + mode.custom_mode = (uint)modein; + break; + default: + return false; + } + return true; + } + + private byte[] SetParam2Async(string paramname, float value) + { + // param type is set here, however it is always sent over the air as a float 100int = 100f. + var req = new MAVLink.mavlink_param_set_t { target_system = 1, target_component = 1, param_type = 4 }; + + byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname); + + Array.Resize(ref temp, 16); + req.param_id = temp; + req.param_value = (value); + + 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(); + 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]; + led.custom_bytes[0] = 255; + led.custom_bytes[1] = 255; + led.custom_bytes[2] = 255; + led.custom_bytes[3] = 3; + + return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led); + } + private byte[] DoThrowoutCommandAsync() + { + 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 = 1; //长度是1个字节 + led.custom_bytes = new byte[24]; + led.custom_bytes[0] = (byte)10; //10代表抛物 + return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led); + } + + + private byte[] DoTaskLEDCommandAsync(int ledmode, float ledInterval, int ledtimes, byte cR,byte cG,Byte cB) + { + //新版固件ledInterval间隔单位秒，传输为一个字节无法传输小数，所以*10改为100ms，可传输一位小数 + byte LEDInterval = (byte)(ledInterval * 10);// (byte)(10 / ledrate); + 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 = 6; //测试LED灯光 + led.custom_bytes = new byte[24]; + led.custom_bytes[0] = cR; + led.custom_bytes[1] = cG; + led.custom_bytes[2] = cB; + led.custom_bytes[3] =(byte)ledmode; + led.custom_bytes[4] = LEDInterval; + led.custom_bytes[5] = (byte)ledtimes; + return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led); + } + + + + private byte[] BatchCopterIdData(IEnumerable copters) + { + byte[] BatchByte = null; + List copterList = copters.OrderBy(c => int.Parse(c.Id)).ToList(); + for (int i = 0; i < copterList.Count; i++) + { + byte[] tempByte; + int id = int.Parse(copterList[i].Id); + int count = 1; + while (i + 1 < copterList.Count && int.Parse(copterList[i + 1].Id) == id + count) + { + count++; + i++; + } + + if (count == 1) + { + short curId = (short)(0 << 12 ^ id); + tempByte = BitConverter.GetBytes(curId); + } + else + { + short beginId = (short)(2 << 12 ^ id); + tempByte = BitConverter.GetBytes(beginId); + + short numCount = (short)(4 << 12 ^ count); + tempByte = tempByte.Concat(BitConverter.GetBytes(numCount)).ToArray(); + } + + if (BatchByte == null) + BatchByte = tempByte; + else + BatchByte = BatchByte.Concat(tempByte).ToArray(); + } + return BatchByte; + } + + private void GetCopterIds(IEnumerable copters, out short copterId, out byte[] batchPacket) + { + copterId = 0; + batchPacket = null; + + if (copters != null) + { + if (copters.Count() == 1) + copterId = short.Parse(copters.FirstOrDefault().Id); + else + { + batchPacket = BatchCopterIdData(copters); + } + } + } + + private async Task RtcmLoop() + { + while (rtcm_run) + { + byte[] sendDate = new byte[0]; + lock (lock_rtcm) + { + if (rtcm_packets.Count >= 3 || + (starttime && waitRtcmTime.AddMilliseconds(200) <= DateTime.Now)) + { + if (rtcm_packets.Count > 0) + { + Windows.Messages.Message.Show($"rtcm_packets.Count = {rtcm_packets.Count}"); + for (int i = 0; i < rtcm_packets.Count; i++) + { + sendDate = sendDate.Concat(rtcm_packets[i]).ToArray(); + } + rtcm_packets.Clear(); + starttime = false; + } + } + } + + if (sendDate != null && sendDate.Length > 0) + { + Windows.Messages.Message.Show($"{DateTime.Now.ToString("HH:mm:ss:fff")} 通信数据长度 = {sendDate.Length}"); + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, sendDate).ConfigureAwait(false); + } + + await Task.Delay(10); + } + } + + /////////////////////////////////////////////////////////////公开输出函数 + /// + /// 设置通道 + /// + /// + /// + /// + /// + /// + /// + + + + public async Task SetChannelsAsync(IEnumerable copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null) + { + 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) + { + await Task.WhenAll(copters.Select(async copter => + await copter.SetChannelsAsync(ch2:ch2))).ConfigureAwait(false); + + } + public async Task SetChannelsAsync_CMod(IEnumerable copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = SetChannels(ch1, ch2, ch3, ch4); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + + /// + /// 开始写任务 + /// + /// 飞机ID + /// 任务列表 + /// + /// + + public async Task WriteMissionListAsync(ICopter copter, List missions) + { + if (UseTransModule) + return await WriteMissionListAsync_CMod(short.Parse(copter.Id), missions); + else return await WriteMissionListAsync_Single(copter, missions); + + } + + public async Task WriteMissionListAsync_Single(ICopter copter, List missions) + { + return await copter.WriteMissionListAsync(missions); + } + + public async Task WriteMissionListAsync_CMod(short id, List missions) + { + missions.Insert(0, PLCopter.PRE_TAKE_OFF_MISSION); + List mission_list = new List(); + foreach (IMission mission in missions) + { + var frame = mission.Command == FlightCommand.Waypoint && mission.Sequence == 0 ? MAVLink.MAV_FRAME.GLOBAL : MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT; + + var req = new MAVLink.mavlink_mission_item_t(); + + req.target_system = 1; + req.target_component = 1; + + req.command = (byte)mission.Command; + + req.current = 0; + req.autocontinue = 1; + + req.frame = (byte)frame; + if (mission.Command == FlightCommand.LEDControl) + { + req.x = mission.R; + req.y = mission.G; + req.z = mission.B; + } + else + { + req.x = (float)mission.Latitude; + req.y = (float)mission.Longitude; + req.z = mission.Altitude; + } + + req.param1 = mission.Param1; + req.param2 = mission.Param2; + req.param3 = mission.Param3; + req.param4 = mission.Param4; + + req.seq = mission.Sequence; + mission_list.Add(req); + } + + bool result = await MissionPacket(id, (byte)MavComm.COMM_DOWNLOAD_MODE, mission_list.ToArray()); + + lock(MissinLocker) + { + if (missionWriteState.ContainsKey(id)) + { + missionWriteState[id].StateReturn = MissionErrorId; + missionWriteState[id].ErrorCode = ErrorCode; + missionWriteState[id].SendAchieved = result; + missionWriteState[id].WriteSucceed = false; + } + else + { + CommWriteMissinState state = new CommWriteMissinState(result); + state.StateReturn = MissionErrorId; + state.ErrorCode = ErrorCode; + missionWriteState.Add(id, state); + } + return result; + } + } + + /// + /// 开始执行任务 + /// + /// 开始执行的小时 + /// 开始执行的分钟 + /// 开始执行的秒 + /// 任务原点经度 + /// 任务原点纬度 + /// + public async Task DoMissionStartAsync(IEnumerable copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) + { + if (UseTransModule) + await DoMissionStartAsync_CMod(copters,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) + { + + foreach (var vcopter in copters) + { + await vcopter.MissionStartAsync(hour_utc, + minute_utc, + second_utc, + Missionlng, + Missionlat + ); + } + + } + + + public async Task DoMissionStartAsync_CMod(IEnumerable copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) + { + + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, -1); + short copterId = 0; + byte[] batchPacket = null; + //部分开始任务 + if (copters!=null) + GetCopterIds(copters, out copterId, out batchPacket); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + + + /// + /// 暂停任务 + /// + public async Task DoMissionPauseAsync() + { + 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($"未开发完成！！"); + } + + /// + /// 恢复任务 + /// + public async Task DoMissionResumeAsync(int hour_utc, int minute_utc, int second_utc) + { + 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($"未开发完成！！"); + } + + /// + /// 解锁 + /// + /// 要解锁的飞机 + /// + public async Task UnlockAsync(IEnumerable copters = null) + { + 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); //ALT_HOLD LOITER + + byte[] packet3 = DoARMAsync(true); + + byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray(); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + } + else + { + IEnumerable vcopters = copters; + if (vcopters == null) vcopters = _allcopters; + if (vcopters != null) + { + foreach (var vcopter in vcopters) + await vcopter.UnlockAsync(); + } + } + } + + + /// + /// 抛物 + /// + /// 要解锁的飞机 + /// + public void ThrowoutAsync(IEnumerable copters = null) + { + if (UseTransModule) + { + Task.Run(async () => + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = DoThrowoutCommandAsync(); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + }); + } + else + { + /* + Task.Run(async () => + { + foreach (var vcopter in copters) + await vcopter.LEDAsync(); + }); + */ + + } + } + + /// + /// LED闪烁白灯 + /// + /// 要闪烁的飞机 + /// + public void LED_FlickerAsync(IEnumerable copters = null) + { + if (UseTransModule) + { + 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); + }); + } + else + { + Task.Run(async () => + { + foreach (var vcopter in copters) + await vcopter.LEDAsync(); + }); + + } + + } + + /* + + private int ledmode = 0; //灯光模式 0常亮 1闪烁 2随机闪烁(RGB无意义) + private float ledInterval = 0; //间隔 单位秒 最小0.1 + private int ledtimes = 0; //次数 (预留、暂取消其意义) + private string ledRGB = "FFFFFF"; //灯光颜色 + + */ + + public void LED_TaskAsync( int ledmode, float ledInterval, int ledtimes, string ledRGB, ICopter copter,bool oldver=true ) + { + if (UseTransModule) + { + Task.Run(async () => + { + Color rightColor = (Color)ColorConverter.ConvertFromString("#" + ledRGB); + byte[] packet = DoTaskLEDCommandAsync(ledmode, ledInterval, ledtimes, (byte)rightColor.R, (byte)rightColor.G, (byte)rightColor.B); + await WriteCommPacketAsync(short.Parse(copter.Id), MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + }); + } + + + } + + + + /// + /// 设置参数 + /// + /// + /// + /// + /// + public async Task SetParamAsync(string paramname, float value, IEnumerable copters = null) + { + 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 + { + IEnumerable vcopters = copters; + if (vcopters == null) vcopters = _allcopters; + if (vcopters != null) + { + foreach (var vcopter in vcopters) + await vcopter.SetParamAsync(paramname, value); + } + return 0; + } + } + + /// + /// 广播设置多个参数 + /// + /// + /// + public async Task SetMultipleParamAsync(params string[] param) + { + if (param.Length % 2 == 1) + return 0; + if (UseTransModule) + { + + 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 + { + 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]); + // foreach (var vcopter in Copters ) + // await vcopter.SetParamAsync(paramname, value); + + } + + + + + return 0; + } + } + + + public void SetAllCoptersForWifi(IEnumerable copters ) + { + + _allcopters = copters; + + } + + + + /// + /// 读取参数 + /// + /// + /// + /// + /// + + + public async Task ReadParamAsnyc(string paramname, IEnumerable copters = null) + { + 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); + } + else + { + Windows.Messages.Message.Show($"未开发完成！！"); + + } + } + + /// + /// 起飞测试 + /// + /// 起飞高度 + /// + /// + public async Task TakeOffAsync(float alt, IEnumerable copters = null) + { + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet1 = SetModeAsync(FlightMode.GUIDED); + + byte[] packet2 = SetChannels(1500, 1500, 1500, 1500); + + 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); + } + else + { + foreach (var vcopter in copters) + await vcopter.TakeOffAsync(); + + } + } + + /// + /// 返航 + /// + /// + /// + public async Task ReturnToLaunchAsync(IEnumerable copters = null) + { + 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); + } + else + { + foreach (var vcopter in copters) + await vcopter.ReturnToLaunchAsync (); + + } + + } + + + + + /// + /// 获取飞机版本 + /// + /// + /// + public async Task GetVersionsAsync(IEnumerable copters = null) + { + 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); + } + else + { + + Windows.Messages.Message.Show($"未开发完成！！"); + } + + } + + public async Task DoMissionEmergencyRetAsync(PLLocation takeoffcentloc, PLLocation taskcentloc, float mindistance,int rettime,bool descending) + { + Int32 param7 = rettime << 16; + //低16位中的第一位表示是否直接降低高度 + if (descending) + param7 = param7 | 1; + if (UseTransModule) + { + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, + (float)takeoffcentloc.Latitude * 1000, + (float)takeoffcentloc.Longitude * 1000,//起飞高度不用传=0 + (float)taskcentloc.Latitude * 1000, + (float)taskcentloc.Longitude * 1000, + taskcentloc.Altitude, + mindistance, param7 + ); + short copterId = 0; + byte[] batchPacket = null; + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + else + { + Windows.Messages.Message.Show($"未开发完成！！"); + } + + + + } + + + public async Task GetCommsumAsync(IEnumerable copters = null) + { + 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 = 1; + 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($"未开发完成！！"); + } + + } + + + + /// + /// 降落 + /// + /// + public async Task LandAsync(IEnumerable copters = null) + { + 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); + } + else + { + IEnumerable vcopters = copters; + if (vcopters == null) vcopters = _allcopters; + if (vcopters != null) + { + foreach (var vcopter in vcopters) + await vcopter.LandAsync(); + } + + } + + } + + /// + /// 加锁 + /// + /// + /// + public async Task LockAsync(IEnumerable copters = null) + { + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet1 = SetChannels(1500, 1500, 1100, 1500); + + byte[] packet2 = DoARMAsync(false); + + byte[] data = packet1.Concat(packet2).ToArray(); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + } + else + { + IEnumerable vcopters = copters; + if (vcopters == null) vcopters = _allcopters; + if (vcopters != null) + { + foreach (var vcopter in vcopters) + await vcopter.LockAsync(); + } + } + + } + + /// + /// 测试电机 + /// + /// + /// + public async Task MotorTestAsync(IEnumerable copters = null) + { + 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); + + byte[] data = data1.Concat(data2).Concat(data3).Concat(data4).ToArray(); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket); + } + else + { + if (copters != null) + foreach (var vcopter in copters) + { + await vcopter.MotorTestAsync(1); + await vcopter.MotorTestAsync(2); + await vcopter.MotorTestAsync(3); + await vcopter.MotorTestAsync(4); + } + + } + + } + + /// + /// 切换悬停模式 + /// + /// + public async Task HoverAsync(IEnumerable copters = null) + { + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet1 = SetChannels(1500, 1500, 1500, 1500); + + byte[] packet2 = SetModeAsync(FlightMode.LOITER); + + byte[] data = packet1.Concat(packet2).ToArray(); + await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + } + else + { + if (copters != null) + foreach (var vcopter in copters) + await vcopter.HoverAsync(); + + } + + } + + /// + /// 切换手动模式 + /// + /// + /// + public async Task FloatAsync(IEnumerable copters = null) + { + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] packet = SetModeAsync(FlightMode.ALT_HOLD); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + else + { + if (copters!=null) + foreach (var vcopter in copters) + await vcopter.FloatAsync(); + + } + } + + /// + /// 校准加速计 + /// + /// + /// + public async Task DoStartPreflightCompassAsync(IEnumerable copters = null) + { + if (UseTransModule) + { + 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, batchPacket).ConfigureAwait(false); + } + else + { + if (copters != null) + foreach (var vcopter in copters) + await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 0, 0, 0, 0, 1, 0, 0); + + } + } + + /// + /// 校准加速计下一步 + /// + /// + /// + 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; + + if (UseTransModule) + { + byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_ACK, req); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + else + { + if (copters != null) + foreach (var vcopter in copters) + { + await vcopter.DoCommandAckAsync(1, 1); + } + } + } + + /// + /// 校准指南针 + /// + /// + /// + public async Task DoCalibrationCompassAsync(IEnumerable copters = null) + { + if (UseTransModule) + { + 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, batchPacket).ConfigureAwait(false); + } + else + { + if (copters != null) + foreach (var vcopter in copters) + await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.DO_START_MAG_CAL, 0, 1, 1, 0, 0, 0, 0); + + } + } + + /// + /// 放弃校准指南针 + /// + /// + /// + public async Task DoCancelCalibrationCompassAsync(short copterId) + { + if (UseTransModule) + { + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_CANCEL_MAG_CAL, 0, 0, 1, 0, 0, 0, 0); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + } + else + { + Windows.Messages.Message.Show($"未开发完成！！"); + } + } + /// + /// 确认校准磁罗盘 + /// + /// + /// + public async Task DoAcceptCalibrationCompassAsync(short copterId) + { + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_ACCEPT_MAG_CAL, 0, 0, 1, 0, 0, 0, 0); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + } + + + byte rtcm_tmpser = 0; + + + public void BroadcastGpsDataAsync(byte[] data, ushort length) + { + //广播发送RTCM数据采用专用数据可以一次发180个字节 + if (!Recomisopen) + return; + + 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 + { + RecomPort.Write(packet, 0, packet.Length); + } + catch (Exception ex) + { + Windows.Messages.Message.Show("转发端口发送失败" + ex.Message); + } + } + inject_seq_no++; + + } + + + + + /// + ///广播Rtk //-------------使用中的RTK广播函数------------ + /// + /// + /// + /// + public async Task InjectGpsDataAsync(byte[] data, ushort length) + { + //由于通讯模块限制一次只能发110个字节 + if (UseTransModule) + { + 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++) + { + 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_tmpser++; + gps.len = (byte)datalen; + gps.target_component = rtcm_tmpser++; + gps.target_system = 1; + + // Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen ); + + byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len); + + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + + await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 + + //重发一次，有序列号(target_component)飞机可以检测出来重复接收的 + //需要新固件支持 + // await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + // await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 + + /* + //重发一次，有序列号(target_component)飞机可以检测出来重复接收的--需要新固件支持 + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 + */ + + + } + } + else + { + if (_allcopters != null) + foreach (var vcopter in _allcopters) + await vcopter.InjectGpsDataAsync(data, length); + } + } + + int inject_seq_no = 0; + + /// + /// 插入RTK数据 + /// + /// + /// + /// + public async Task InjectGpsRTCMDataAsync(byte[] data, int length) + { + MAVLink.mavlink_gps_rtcm_data_t gps = new MAVLink.mavlink_gps_rtcm_data_t(); + var msglen = 180; //飞机端是180，之前也是180和通讯盒可能也有关系 + + // if (length > msglen * 4) + // log.Error("Message too large " + length); + + // number of packets we need, including a termination packet if needed + var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1; + + // if (nopackets>1) + // Plane.Windows.Messages.Message.Show(" RTCM_Mut: " + length); + + 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 = GeneratePacket(MAVLink.MAVLINK_MSG_ID_GPS_RTCM_DATA, gps); + + // packet[2] + //Plane.Windows.Messages.Message.Show("["+ packet[2]+"]RTCM:"+ gps.len); + + // Console.WriteLine("RTCM:" + gps.len+ "[" + data[0]+","+ data[1] + "," + data[2] + "," + data[3] + "," + data[4]+"]"); + + //Windows.Messages.Message.Show($"{DateTime.Now.ToString("HH:mm:ss:fff")} 单次长度 = {packet.Length}"); + //老版本在这发送，新版本在StartRtcmLoop里面一次性打包4个180字节发送了 + await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + /* //长RTCP数据发送还没开发完 + lock (lock_rtcm) + { + rtcm_packets.Add(packet); + if (rtcm_packets.Count == 1) + { + waitRtcmTime = DateTime.Now; + starttime = true; + } + } + */ + + } + } + + + + /// + /// 开始循环检测Rtcm数量 + /// + /// + public async Task StartRtcmLoop() + { + rtcm_run = true; + waitRtcmTime = DateTime.Now; + await Task.Factory.StartNew(RtcmLoop); + } + + /// + /// 结束循环检测Rtcm数量 + /// + /// + public async Task CloseRtcmLoop() + { + rtcm_run = false; + await Task.Delay(1); + } + + public void CloseResendRtcmserial() + { + if (Recomisopen) + { + RecomPort.Close(); + RecomPort.Dispose(); + } + Recomisopen = false; + } + public bool OpenResendRtcmserial(string reserialport) + { + + RecomPort= new SerialPort(reserialport); + RecomPort.BaudRate = 57600; + RecomPort.Parity = Parity.None; + RecomPort.StopBits = StopBits.One; + RecomPort.DataBits = 8; + RecomPort.Handshake = Handshake.None; + + try + { + RecomPort.Open(); + Recomisopen = true; + } + catch (Exception ex) + { + // Alert.Show("转发端口打开失败" + ex.Message); + } + + return Recomisopen; + } + + + + + /// + /// 产生RTK包 + /// + /// + /// + /// + /// + /// + public byte[] GenerateRTKPacketAsync(byte messageType, TMavlinkPacket indata, int rtklength) + { + byte[] data; + int rtkdataleng = rtklength + 3; + + data = MavlinkUtil.StructureToByteArray(indata); + + + byte[] packet = new byte[rtkdataleng + 6 + 2]; + + packet[0] = MAVLink.MAVLINK_STX; + packet[1] = (byte)(rtkdataleng); + packet[2] = (byte)packetcount; + packetcount++; + packet[3] = 255; // this is always 255 - MYGCS + packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; + packet[5] = messageType; + + int i = 6; + foreach (byte b in data) + { + if (i < rtkdataleng + 6) + packet[i] = b; + else break; + i++; + } + + ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6); + + checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], 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; + + return packet; + } + + + } + +} diff --git a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs index 45a5958..52777ec 100644 --- a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs @@ -24,7 +24,7 @@ namespace Plane.Copters /// /// 更新虚拟飞行器状态的时间间隔，单位为毫秒。 /// - static private int UPDATE_INTERVAL =100; //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架 + static private int UPDATE_INTERVAL = 100; //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架 static private int UPDATE_INTERVAL_TEMP = 50; /// @@ -35,7 +35,7 @@ namespace Plane.Copters /// /// 高速模式下，在一个更新间隔中的最大移动距离。 /// - private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4; //MAX_MOVE_IN_INTERVAL*4 + private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4; //MAX_MOVE_IN_INTERVAL*4 @@ -101,7 +101,7 @@ namespace Plane.Copters // 目标点相对于开始位置的方向。 - private double _direction; + private double _direction; /// /// FlyTo 的目标纬度。 @@ -131,17 +131,26 @@ namespace Plane.Copters private System.Drawing.ColorConverter rgbconverter; + private PLLocation _takeoffcentloc; + private PLLocation _taskcentloc; + private float _mindistance; + private int _rettime; + private bool _descending; + int Emergency_Retstatus = 0; + //返航路径第一个航点 + PLLocation Emergency_firstloc; + /// /// 使用 创建 实例。 /// public FakeCopter() : this(SynchronizationContext.Current) { - + } - new public int sim_update_int + new public int sim_update_int { get { return UPDATE_INTERVAL; } set @@ -151,9 +160,9 @@ namespace Plane.Copters //快速模式最大移动距离 MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4; //速度缩放后快速速度距离 - _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST* _speedScale; + _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST * _speedScale; //速度缩放后速度距离 - _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL* _speedScale; + _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL * _speedScale; Set(nameof(sim_update_int), ref UPDATE_INTERVAL, value); } } @@ -207,19 +216,19 @@ namespace Plane.Copters // 持续计算并更新虚拟飞行器的状态。 // Task.Run(async () => - Task.Factory.StartNew(async () => + Task.Factory.StartNew(async () => - { + { while (_isRunning) { Update(); await TaskUtils.Delay(UPDATE_INTERVAL).ConfigureAwait(false); } } - , TaskCreationOptions.LongRunning - ); - - + , TaskCreationOptions.LongRunning + ); + + ++HeartbeatCount; GpsFixType = GpsFixType.Fix3D; GpsHdop = 1; @@ -281,6 +290,50 @@ namespace Plane.Copters IsCheckingConnection = false; return TaskUtils.CompletedTask; } + private void BuildPath() + { + float taralt; + //返航高度参数 + float retalt = 15; + + if (_descending) + { + if (Altitude <= retalt) taralt = Altitude; + else + { + float maxalt = Altitude - retalt; + //降低一个随机高度，随机高度介于0和maxalt之间，也就是目标高度不要低于retalt, 防止飞机在同一高度飞行 + taralt = (float)(Altitude - (new Random().NextDouble() * maxalt)); + } + Emergency_firstloc = new PLLocation(this.Latitude,this.Longitude, taralt); + } + else + { + //计算当前位置和起飞点的距离 + float dis = (float)this.CalcDistance(TakeOffPoint.Latitude, TakeOffPoint.Longitude,Altitude); + //随机水平飞行距离 + float maxdis = (float)new Random().NextDouble() * dis; + + + + + } + + + } + public Task EmergencyRetAsync(PLLocation takeoffcentloc, PLLocation taskcentloc, float mindistance, int rettime, bool descending) + { + _takeoffcentloc = takeoffcentloc; + _taskcentloc = taskcentloc; + _mindistance = mindistance; + _rettime = rettime; + _descending = descending; + Emergency_Retstatus = 0; + //计算返航路径 + BuildPath(); + Mode = FlightMode.EMERG_RTL; + return TaskUtils.CompletedTask; + } protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0) { @@ -497,8 +550,12 @@ namespace Plane.Copters _takeOffTargetAltitude = (int)alt; await TakeOffAsync().ConfigureAwait(false); } + DateTime MissionStartTime; public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) { + MissionStartTime = DateTime.Now; + TakeOffPoint = new PLLocation(Missionlat, Missionlng, 0); + return TaskUtils.CompletedTask; } @@ -764,6 +821,49 @@ namespace Plane.Copters case FlightMode.TOY: break; + //紧急返航 + case FlightMode.EMERG_RTL: + switch(Emergency_Retstatus) + { + case 0: //等待返航 + if ((DateTime.Now- MissionStartTime).TotalSeconds > _rettime) + { + Emergency_Retstatus = 1; + //平飞或降落随机距离 + FlyToCoreAsync(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude); + //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来 + Mode = FlightMode.EMERG_RTL; + } + + + break; + case 1: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空 + + UpdateWithDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude); + if (ReachedDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude)) + { + FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude); + //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来 + Mode = FlightMode.EMERG_RTL; + Emergency_Retstatus = 2; + } + + break; + case 2: //返航阶段二:等待到达起飞点上空,然后降落 + + UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude); + if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude)) + { + FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude); + //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来 + Mode = FlightMode.LAND; + Emergency_Retstatus = 3; + } + break; + case 3: //降落 + break; + } + break; default: break; diff --git a/PlaneGcsSdk_Shared/Copters/FlightMode.cs b/PlaneGcsSdk_Shared/Copters/FlightMode.cs index 3d4b0d4..77cdd06 100644 --- a/PlaneGcsSdk_Shared/Copters/FlightMode.cs +++ b/PlaneGcsSdk_Shared/Copters/FlightMode.cs @@ -1,84 +1,86 @@ -using System; - -namespace Plane.Copters -{ -#if PRIVATE - public -#else - internal -#endif - enum FlightMode - { - // 王海，20150608：不可将以下枚举项重命名。 - - STABILIZE = 0, // hold level position - - ACRO = 1, // rate control // 王海, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/ - - ALT_HOLD = 2, // AUTO control - - AUTO = 3, // AUTO control - - GUIDED = 4, // AUTO control - - LOITER = 5, // Hold a single location - - RTL = 6, // AUTO control - - CIRCLE = 7, - - POSITION = 8, // 王海, 20160205, 位置模式：http://copter.ardupilot.cn/wiki/POSITION-mode/ - - LAND = 9, // AUTO control - - OF_LOITER = 10, // 王海, 20160205, 光流悬停模式：http://copter.ardupilot.cn/wiki/loiter-mode/ 底部。 - - TOY = 11 - } - - internal static class FightModeExtensions - { - public static string GetModeString(this FlightMode flightMode) - { - switch (flightMode) - { - case FlightMode.ALT_HOLD: - return "ALT HOLD"; - - case FlightMode.POSITION: - return "POS HOLD"; - - default: - return Enum.GetName(typeof(FlightMode), flightMode); - } - } - - public static bool NeedGps(this FlightMode flightMode) - { - switch (flightMode) - { - case FlightMode.AUTO: - case FlightMode.GUIDED: - case FlightMode.LOITER: - case FlightMode.RTL: - case FlightMode.CIRCLE: - case FlightMode.LAND: - case FlightMode.POSITION: - default: - return true; - - case FlightMode.STABILIZE: - case FlightMode.ACRO: - case FlightMode.ALT_HOLD: - case FlightMode.OF_LOITER: - case FlightMode.TOY: - return false; - } - } - - internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode) - { - return (PlaneCopter.ac2modes)flightMode; - } - } -} +using System; + +namespace Plane.Copters +{ +#if PRIVATE + public +#else + internal +#endif + enum FlightMode + { + // 王海，20150608：不可将以下枚举项重命名。 + + STABILIZE = 0, // hold level position + + ACRO = 1, // rate control // 王海, 20160205, 特技模式：http://copter.ardupilot.cn/wiki/acro-mode/ + + ALT_HOLD = 2, // AUTO control + + AUTO = 3, // AUTO control + + GUIDED = 4, // AUTO control + + LOITER = 5, // Hold a single location + + RTL = 6, // AUTO control + + CIRCLE = 7, + + POSITION = 8, // 王海, 20160205, 位置模式：http://copter.ardupilot.cn/wiki/POSITION-mode/ + + LAND = 9, // AUTO control + + OF_LOITER = 10, // 王海, 20160205, 光流悬停模式：http://copter.ardupilot.cn/wiki/loiter-mode/ 底部。 + + TOY = 11, + + EMERG_RTL=12, //紧急返航模式 + } + + internal static class FightModeExtensions + { + public static string GetModeString(this FlightMode flightMode) + { + switch (flightMode) + { + case FlightMode.ALT_HOLD: + return "ALT HOLD"; + + case FlightMode.POSITION: + return "POS HOLD"; + + default: + return Enum.GetName(typeof(FlightMode), flightMode); + } + } + + public static bool NeedGps(this FlightMode flightMode) + { + switch (flightMode) + { + case FlightMode.AUTO: + case FlightMode.GUIDED: + case FlightMode.LOITER: + case FlightMode.RTL: + case FlightMode.CIRCLE: + case FlightMode.LAND: + case FlightMode.POSITION: + default: + return true; + + case FlightMode.STABILIZE: + case FlightMode.ACRO: + case FlightMode.ALT_HOLD: + case FlightMode.OF_LOITER: + case FlightMode.TOY: + return false; + } + } + + internal static PlaneCopter.ac2modes ToAC2Mode(this FlightMode flightMode) + { + return (PlaneCopter.ac2modes)flightMode; + } + } +}