// #define LOG_SEQUENCE_NUMBER //#define LOG_PACKETS using Plane.Protocols; using Plane.Communication; using TaskTools.Utilities; using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Threading; using System.Diagnostics; using System.IO; using System.Threading.Tasks; namespace Plane.Copters { #if PRIVATE public #else internal #endif partial class PlaneCopter { uint iErrorData; ulong iDataCount; public bool IsConnected { get { return Connection.IsOpen; } } public string Id { get; set; } public byte mavlinkversion { get; set; } public MAVLink.MAV_TYPE aptype { get; set; } public MAVLink.MAV_AUTOPILOT apname { get; set; } private bool CommOK = true; public byte sysid { get; set; } public byte compid { get; set; } public byte recvpacketcount { get; set; } public bool armed { get; set; } public bool failsafe { get; set; } public uint mode { get; set; } public float battery_voltage { get; set; } public byte battery_remaining { get; set; } public float current_battery { get; set; } public float freemem { get; set; } public float brklevel { get; set; } public bool useLocation { get; set; } public float gpsalt { get; set; } public byte gpsstatus { get; set; } public float gpshdop { get; set; } public byte satcount { get; set; } public float groundspeed { get; set; } public float groundcourse { get; set; } public double lat { get; set; } public double lng { get; set; } public float nav_roll { get; set; } public float nav_pitch { get; set; } public short nav_bearing { get; set; } public short target_bearing { get; set; } public float wp_dist { get; set; } public float alt_error { get; set; } public float aspd_error { get; set; } public float xtrack_error { get; set; } public float roll { get; set; } public float pitch { get; set; } public float yaw { get; set; } public short heading { get; set; } public float airspeed { get; set; } public float alt { get { return altorigin - altoffset; } } public float altoffset = 0; public float altorigin { get; private set; } = 0; public float ch3percent { get; set; } public ushort ch1in { get; set; } public ushort ch2in { get; set; } public ushort ch3in { get; set; } public ushort ch4in { get; set; } public ushort ch5in { get; set; } public ushort ch6in { get; set; } public ushort ch7in { get; set; } public ushort ch8in { get; set; } public float rxrssi { get; set; } public byte remrssi { get; set; } public uint timebootms { get; set; } private int DataTimeOut = 0; public Dictionary param = new Dictionary(); public Dictionary param_types = new Dictionary(); public IConnection Connection { get; set; } public ushort FlightDistance2D { get; private set; } private SynchronizationContext _uiSyncContext; private bool _autoSendHeartbeat = true; private bool _autoRequestData = true; public PlaneCopter(IConnection connection, SynchronizationContext uiSyncContext, bool autoSendHeartbeat = true, bool autoRequestData = true) { this.Connection = connection; _uiSyncContext = uiSyncContext; _autoSendHeartbeat = autoSendHeartbeat; _autoRequestData = autoRequestData; } public async Task ConnectAsync() { //sysid = 2; SendReq = false; CommOK = true; await Connection.OpenAsync().ConfigureAwait(false); StartCommunication(); } public async Task DisconnectAsync() { #if DEBUG && !NETFX_CORE && LOG_SEQUENCE_NUMBER File.AppendAllText($"SequenceNumbers_{Id}.log", _sequenceLog.ToString()); #endif await StopDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0); CommOK = false; Connection.Close(); } public async Task GetCopterDataAsync() { //停止所有数据流 await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0).ConfigureAwait(false); //需要电池电压，电流，GPS数据 await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, 5).ConfigureAwait(false); //10 1电池电压、电流、电量,152-内存、62-导航控制输出、24-GPS数据、42-航点数据 //不需要姿态信息 // await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA1, 1).ConfigureAwait(false); //5 请求 飞行姿态 roll、pitch、yaw、rollspeed、pitchspeed、yawspeed--- 30 //需要高度和机头朝向数据 await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA2, 5).ConfigureAwait(false); //10 请求vfr airspeed(空速)、groundspeed(地速)、alt(高度)、climb(爬升率)、heading(头朝向)、throttle(实际油门) ---74 //不需要通道数据 //await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 1).ConfigureAwait(false); //1 请求 通道数据 //不需要原始数据 // await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, ratesensors).ConfigureAwait(false); // 请求 传感器原始值 /* //已经停止所有数据了，不用单独停止 // Disable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER, 0).ConfigureAwait(false); // Disable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.POSITION, 0).ConfigureAwait(false); // Disable EXTRA3 (Dependent on the autopilot |) await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA3, 0).ConfigureAwait(false); // Disable Plane Stream await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.GHOST_STREAM, 0).ConfigureAwait(false); // Disable RAW_SENSORS await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 0).ConfigureAwait(false); // // 5 Disable MSG_ATTITUDE roll、pitch、yaw、rollspeed、pitchspeed、yawspeed-- - 30 await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA1, 0).ConfigureAwait(false); // */ } private async void StartCommunication() { try { var writingTask = _autoSendHeartbeat ? StartSendingHeartbeatsAsync() : TaskUtils.CompletedTask; var readingTask = StartReadingPacketsAsync(); await Task.WhenAll(writingTask, readingTask).ConfigureAwait(false); } catch (Exception ex) { RaiseMessageCreated(ex.ToString()); } } #if DEBUG && LOG_PACKETS private Stopwatch _sendHeartbeatStopwatch = new Stopwatch(); private TimeSpan _sendHeartbeatTimeSpan; #endif public async Task StartSendingHeartbeatsAsync() { await Task.Run(async () => { while (CommOK) { if (!IsConnected) { break; } // if (heatbeatSend.Second != DateTime.Now.Second) if (heatbeatSend.AddMilliseconds(1000) <= DateTime.Now) { var htb = new MAVLink.mavlink_heartbeat_t() { type = (byte)MAVLink.MAV_TYPE.GCS, autopilot = (byte)MAVLink.MAV_AUTOPILOT.GENERIC, mavlink_version = 3, }; await SendPacketAsync(htb).ConfigureAwait(false); #if DEBUG && LOG_PACKETS if (!_sendHeartbeatStopwatch.IsRunning) { _sendHeartbeatStopwatch.Start(); } else { var elapsed = _sendHeartbeatStopwatch.Elapsed; Debug.WriteLine(elapsed - _sendHeartbeatTimeSpan, "--------------------------发送心跳间隔"); _sendHeartbeatTimeSpan = elapsed; } #endif heatbeatSend = DateTime.Now; DataTimeOut += 1; if (DataTimeOut > 3) { bInitChannel = false; SendReq = false; } } else { await Task.Delay(100).ConfigureAwait(false); } } }).ConfigureAwait(false); } private async Task StartReadingPacketsAsync() { await Task.Run(async () => { while (CommOK) { if (!IsConnected) { RaiseEventOnUIThread(ConnectionBroken); break; } var packet = await ReadPacketAsync().ConfigureAwait(false); if (packet != null) { PacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet)); var handled = true; Debug.WriteLine(packet[5],"收到数据类型"); switch (packet[5]) { case MAVLink.MAVLINK_MSG_ID_HEARTBEAT: //0 AnalyzeHeartbeatPacket(packet); break; case MAVLink.MAVLINK_MSG_ID_SYS_STATUS: //1 AnalyzeSysStatusPacket(packet); break; case MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT: //24 AnalyzeGpsRawIntPacket(packet); break; case MAVLink.MAVLINK_MSG_ID_VFR_HUD: //74 AnalyzeVfrHudPacket(packet); break; case MAVLink.MAVLINK_MSG_ID_STATUSTEXT: //253 AnalyzeStatusTextPacket(packet); break; ////以上为基本数据， case MAVLink.MAVLINK_MSG_ID_RADIO: //109 AnalyzeRadioPacket(packet); break; case MAVLink.MAVLINK_MSG_ID_PARAM_VALUE: //22 AnalyzeParamValuePacket(packet); break; case MAVLink.MAVLINK_MSG_ID_MEMINFO: //152 AnalyzeMemInfoPacket(packet); break; case MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT: //42 AnalyzeMissionCurrentPacket(packet); break; case MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT: //33 AnalyzeGlobalPositionIntPacket(packet); break; case MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: //62 AnalyzeNavControllerOutputPacket(packet); break; case MAVLink.MAVLINK_MSG_ID_ATTITUDE: //30 AnalyzeAttitudePacket(packet); break; case MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW: //35 AnalyzeRCChannelsRawPacket(packet); break; case MAVLink.MAVLINK_MSG_ID_RAW_IMU: //没有用27 AnalyzeRawImuPacket(packet); break; case MAVLink.MAVLINK_MSG_ID_SCALED_IMU2: //没有用 116 AnalyzeScaledImu2Packet(packet); break; case MAVLink.MAVLINK_MSG_ID_SENSOR_OFFSETS: //150 AnalyzeSensorOffsetsPacket(packet); break; default: handled = false; UnhandledPacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet)); break; } if (handled) { PacketHandled?.Invoke(this, new PacketReceivedEventArgs(packet)); } } } }).ConfigureAwait(false); } public async Task GetParamsListAsync() { MAVLink.mavlink_param_request_list_t req = new MAVLink.mavlink_param_request_list_t(); req.target_system = sysid; req.target_component = compid; await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req); } public static List> getModesList() { var flightModes = EnumTranslator.Translate(); return flightModes.ToList(); } // 林俊清, 20151029, 由于只在 myComm 循环中读取数据包，因此去掉了锁。 //volatile object readlock = new object(); #if DEBUG && LOG_PACKETS private StringBuilder _log = new StringBuilder(); private Stopwatch _positionStopwatch = new Stopwatch(); private TimeSpan _lastPositionTimeSpan; #endif #if DEBUG && LOG_SEQUENCE_NUMBER private StringBuilder _sequenceLog = new StringBuilder("-----------------------\n"); #endif private byte[] _readBuffer = new byte[263]; /// /// 异步读取数据包。// 林俊清, 20151029, 去掉了锁，不要在除读数据包循环之外的地方调用此方法。 /// /// 表示此异步操作的 实例，其结果为读取到的数据包。 private async Task ReadPacketAsync() { if (!Connection.IsOpen) { return null; } int length = 0; try { if (await Connection.ReadAsync(_readBuffer, 0, 1) == 0) { return null; } if (_readBuffer[0] == MAVLink.MAVLINK_STX) { if (await Connection.ReadAsync(_readBuffer, 1, 5) == 0) { return null; } #if DEBUG && LOG_SEQUENCE_NUMBER // _sequenceLog.Append(DateTime.Now.ToString("HHmmss.fff")).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString())); _sequenceLog.Append(DateTime.Now.ToString()).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString())); #endif // packet length length = 6 + _readBuffer[1] + 2; // header + data + checksum if (await Connection.ReadAsync(_readBuffer, 6, length - 6) == 0) { return null; } } else { #if DEBUG && LOG_PACKETS _log.AppendLine("---------------"); _log.AppendLine("unexpected head: " + _readBuffer[0]); #endif return null; } } catch { return null; } uint crcvalue = BitConverter.ToUInt16(_readBuffer, length - 2); #if DEBUG && LOG_PACKETS _log.AppendLine("---------------"); for (int i = 0; i < 6; i++) { _log.Append(_readBuffer[i]).Append(" "); } _log.AppendLine(); for (int i = 6; i < length - 2; i++) { _log.Append(_readBuffer[i]).Append(" "); } _log.AppendLine(); for (int i = length - 2; i < length; i++) { _log.Append(_readBuffer[i]).Append(" "); } _log.AppendLine(); #endif byte messageType = _readBuffer[5]; #if DEBUG && LOG_PACKETS if (messageType == 33) { if (_positionStopwatch.IsRunning) { var elapsed = _positionStopwatch.Elapsed; Debug.WriteLine((elapsed - _lastPositionTimeSpan).TotalMilliseconds, "-----------------------------------------------position packet"); _lastPositionTimeSpan = elapsed; } else { _positionStopwatch.Start(); } } #endif ushort checksum = MavlinkCRC.crc_calculate(_readBuffer, length - 2); checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum); iDataCount += 1; #if DEBUG && LOG_SEQUENCE_NUMBER _sequenceLog.Append('\t').Append(crcvalue == checksum).AppendLine(); #endif if (crcvalue == checksum) { return _readBuffer; } else { #if DEBUG && LOG_PACKETS _log.AppendLine("crc not passed"); #endif iErrorData += 1; var handler = ErrorDataEvent; if (handler != null) { RunOnUIThread(() => handler(iErrorData, iDataCount, _readBuffer[5])); } return null; } } public MAVState MAV = new MAVState(); /// /// Used to extract mission from log file - both sent or received /// /// packet void getWPsfromstream(ref byte[] buffer) { if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_MISSION_COUNT) { // clear old MAV.wps.Clear(); } if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_MISSION_ITEM) { MAVLink.mavlink_mission_item_t wp = buffer.ByteArrayToStructure(6); if (wp.current == 2) { // guide mode wp MAV.GuidedMode = wp; } else { MAV.wps[wp.seq] = wp; } //Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command); } if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_RALLY_POINT) { MAVLink.mavlink_rally_point_t rallypt = buffer.ByteArrayToStructure(6); MAV.rallypoints[rallypt.idx] = rallypt; //Console.WriteLine("RP # {0} {1} {2} {3} {4}", rallypt.idx, rallypt.lat, rallypt.lng, rallypt.alt, rallypt.break_alt); } if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_FENCE_POINT) { MAVLink.mavlink_fence_point_t fencept = buffer.ByteArrayToStructure(6); MAV.fencepoints[fencept.idx] = fencept; } } /// /// used for outbound packet sending /// public int packetcount = 0; private bool SendReq; #pragma warning disable CS0414 // The field is assigned but its value is never used private bool giveComport; #pragma warning restore CS0414 // The field is assigned but its value is never used public async Task GeneratePacketAsync(byte messageType, TMavlinkPacket indata) { byte[] data; if (mavlinkversion == 3) { data = MavlinkUtil.StructureToByteArray(indata); } else { data = MavlinkUtil.StructureToByteArrayBigEndian(indata); } //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); byte[] packet = new byte[data.Length + 6 + 2]; if (mavlinkversion == 3 || mavlinkversion == 0) { packet[0] = MAVLink.MAVLINK_STX; } else if (mavlinkversion == 2) { packet[0] = (byte)'U'; } packet[1] = (byte)(data.Length); packet[2] = (byte)packetcount; //packet[2] = 35; 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); if (mavlinkversion == 3 || mavlinkversion == 0) { 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; if (IsConnected) { try { await Connection.WriteAsync(packet, 0, i); } catch { // 林俊清, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。 } } } public async Task DoARMAsync(bool armit) { return await DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 0, 0, 0, 0, 0, 0); } public async Task DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout) { return await DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0); } public async Task DoTakeoffAsync() { /// Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| return await DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, -1, 0, 0, 15); } public async Task DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) { giveComport = true; //byte[] buffer; MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t(); req.target_system = sysid; req.target_component = compid; 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; await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); return true; //DateTime start = DateTime.Now; //int retrys = 3; //int timeout = 2000; ////// imu calib take a little while //if (actionid == MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1) //{ // // this is for advanced accel offsets, and blocks execution // return true; //} //else if (actionid == MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION) //{ // retrys = 1; // timeout = 25000; //} //else if (actionid == MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN) //{ // await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); // giveComport = false; // return true; //} //else if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM) //{ // // 10 seconds as may need an imu calib // timeout = 10000; //} //while (true) //{ // if (!(start.AddMilliseconds(timeout) > DateTime.Now)) // { // if (retrys > 0) // { // //log.Info("doAction Retry " + retrys); // await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); // start = DateTime.Now; // retrys--; // continue; // } // giveComport = false; // throw new Exception("Timeout on read - doAction"); // } // buffer = readPacket(); // if (buffer.Length > 5) // { // if (buffer[5] == MAVLink.MAVLINK_MSG_ID_COMMAND_ACK) // { // var ack = buffer.ByteArrayToStructure(6); // if (ack.result == (byte)MAVLink.MAV_RESULT.ACCEPTED) // { // giveComport = false; // return true; // } // else // { // giveComport = false; // return false; // } // } // } //} } public double GetBearing(float lat1,float lng1,float lat2,float lng2) { var latitude1 = Math.PI / 180*lat1; var latitude2 = Math.PI / 180 * lat2; var longitudeDifference = Math.PI / 180*(lng2 - lng1); var y = Math.Sin(longitudeDifference) * Math.Cos(latitude2); var x = Math.Cos(latitude1) * Math.Sin(latitude2) - Math.Sin(latitude1) * Math.Cos(latitude2) * Math.Cos(longitudeDifference); return (180 / Math.PI*(Math.Atan2(y, x)) + 360) % 360; } public async Task SetGuidedModeWPAsync(Locationwp gotohere) { //if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0) // return; //giveComport = true; //try //{ //gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; //老方法无法设置速度 // await SetGuidedWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2); //} //catch { } //giveComport = false; //老方法无法设置速度 await SetWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2); /* TaskTools.HIL.Vector3 targetVelocity = new TaskTools.HIL.Vector3(); float targspeed = (float)2.0; //目标速度 double targetbearing= GetBearing((float)lat, (float)lng, (float)gotohere.lat, (float)gotohere.lng); //目标机头方向 targetVelocity.x = Math.Cos(targetbearing * 180 / Math.PI) * targspeed; //分解速度 targetVelocity.y = Math.Sin(targetbearing * 180 / Math.PI) * targspeed; //分解速度 // await setPositionTargetGlobalInt(true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, // gotohere.lat, gotohere.lng, gotohere.alt, targetVelocity.x, targetVelocity.y, 2.0); await setPositionTargetGlobalInt(true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, gotohere.lat, gotohere.lng, gotohere.alt, 0, 0, 0); */ } public async Task setPositionTargetGlobalInt( bool pos, bool vel, bool acc, MAVLink.MAV_FRAME frame, double lat, double lng, double alt, double vx, double vy, double vz) { MAVLink.mavlink_set_position_target_global_int_t target = new MAVLink.mavlink_set_position_target_global_int_t() { target_system = sysid, target_component = compid, alt = (float)alt, lat_int = (int)(lat * 1e7), lon_int = (int)(lng * 1e7), coordinate_frame = (byte)frame, vx = (float)vx, vy = (float)vy, vz = (float)vz }; target.type_mask = 7 + 56 + 448; if (pos) target.type_mask -= 7; if (vel) target.type_mask -= 56; if (acc) target.type_mask -= 448; await GeneratePacketAsync(MAVLink.SET_POSITION_TARGET_GLOBAL_INT, target); } public async Task SetNewWPAltAsync(Locationwp gotohere) { //givecomport = true; //try //{ gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; //MAVLink.MAV_MISSION_RESULT ans = await SetWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)3); // if (ans != mavlink.mav_mission_result.mav_mission_accepted) // throw new exception("alt change failed"); //} //catch { givecomport = false; /*log.error(ex);*/ throw; } //givecomport = false; } public async Task SetWPAsync(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0) { giveComport = true; MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t(); req.target_system = sysid; req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM req.command = loc.id; req.current = current; req.frame = (byte)frame; req.x = (float)(loc.lat); req.y = (float)(loc.lng); req.z = (float)(loc.alt); req.param1 = loc.p1; req.param2 = loc.p2; req.param3 = loc.p3; req.param4 = loc.p4; req.seq = index; //log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index); // request await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_MISSION_ITEM, req); } public async Task SetChannelsAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null) { //某些情况下ch4设置成0会导致自旋 ushort checkyaw = ch4 ?? ch4in; if ((checkyaw < 1200)|| (checkyaw > 1800)) checkyaw = 1500; //// MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t(); rc.target_component = compid; rc.target_system = sysid; rc.chan1_raw = ch1 ?? ch1in; rc.chan2_raw = ch2 ?? ch2in; rc.chan3_raw = ch3 ?? ch3in; rc.chan4_raw = checkyaw; rc.chan5_raw = ch5 ?? ch5in; rc.chan6_raw = ch6 ?? ch6in; rc.chan7_raw = ch7 ?? ch7in; rc.chan8_raw = ch8 ?? ch8in; await SendPacketAsync(rc); } public async Task SetMobileControlAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null, float? alt = null) { /* MAVLink.mavlink_rc_mobile_control_t rc = new MAVLink.mavlink_rc_mobile_control_t(); rc.target_component = compid; rc.target_system = sysid; rc.chan1_raw = ch1 ?? ch1in; rc.chan2_raw = ch2 ?? ch2in; rc.chan3_raw = ch3 ?? ch3in; rc.chan4_raw = ch4 ?? ch4in; rc.chan5_raw = ch5 ?? ch5in; rc.chan6_raw = ch6 ?? ch6in; rc.chan7_raw = ch7 ?? ch7in; rc.chan8_raw = ch8 ?? ch8in; rc.Yaw = yaw ?? this.yaw; rc.alt = alt ?? this.alt; await SendPacketAsync(rc); */ float ch4yaw; ch4yaw = yaw ?? this.yaw; if (ch4yaw != this.yaw) { ch4yaw = ch4yaw % 360; await DoCommandAsync(MAVLink.MAV_CMD.CONDITION_YAW, ch4yaw, 0, 1, 0, 0, 0, 0); } /* float ch4yaw; MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t(); rc.target_component = compid; rc.target_system = sysid; rc.chan1_raw = ch1 ?? ch1in; rc.chan2_raw = ch2 ?? ch2in; rc.chan3_raw = ch3 ?? ch3in; rc.chan4_raw = 1500; rc.chan5_raw = ch5 ?? ch5in; rc.chan6_raw = ch6 ?? ch6in; rc.chan7_raw = ch7 ?? ch7in; rc.chan8_raw = ch8 ?? ch8in; ch4yaw= yaw ?? this.yaw; rc.chan4_raw =(ushort)ch4yaw; await SendPacketAsync(rc); */ } public async Task GetParamAsync(string paramName, int millisecondsTimeout = Timeout.Infinite) { if (param.ContainsKey(paramName)) param[paramName] = null; var paramId = Encoding.ASCII.GetBytes(paramName); Array.Resize(ref paramId, 16); var req = new MAVLink.mavlink_param_request_read_t { param_id = paramId, param_index = -1, target_component = compid, target_system = sysid }; var stopwatch = Stopwatch.StartNew(); while (true) { for (int ii = 0; ii < 5; ii++) { await SendPacketAsync(req); await Task.Delay(10).ConfigureAwait(false); } int i = 0; for (; i < 5 && (!param.ContainsKey(paramName) || param[paramName] == null); i++) { await Task.Delay(50).ConfigureAwait(false); } if (i < 5) { stopwatch.Stop(); break; } else if (millisecondsTimeout != Timeout.Infinite && stopwatch.ElapsedMilliseconds >= millisecondsTimeout) { stopwatch.Stop(); throw new TimeoutException("The GetParamAsync operation has timed out."); } } return param[paramName].Value; } public async Task SendPacketAsync(TMavlinkPacket indata) { bool validPacket = false; byte a = 0; var packetType = indata.GetType(); foreach (Type ty in MAVLink.MAVLINK_MESSAGE_INFO) { if (ty == packetType) { validPacket = true; await GeneratePacketAsync(a, indata); return; } a++; } if (!validPacket) { RaiseMessageCreated("The packet type is not valid."); //log.Info("Mavlink : NOT VALID PACKET await SendPacketAsync() " + indata.GetType().ToString()); } } public double[] packetspersecond { get; set; } DateTime[] packetspersecondbuild = new DateTime[256]; byte ratesensors = 1; public byte RateSensors { get { return ratesensors; } set { ratesensors = value; } } private DateTime heatbeatSend; private bool bInitChannel = false; private bool bGPSBadHealth; private bool bGyroBadHealth; private bool bAccel; private bool bCompass; private bool bBarometer; public short gx; public short gy; public short gz; public short ax; public short ay; public short az; public short mx; public short my; public short mz; public short gx2; public short gy2; public short gz2; public short ax2; public short ay2; public short az2; public short mx2; public short my2; public short mz2; public float mag_ofs_x; public float mag_ofs_y; public float mag_ofs_z; public float mag_declination; public int raw_press; public int raw_temp; public float gyro_cal_x; public float gyro_cal_y; public float gyro_cal_z; public float accel_cal_x; public float accel_cal_y; public float accel_cal_z; public ushort param_total; public ushort WpCount; public async Task RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) { double pps = 0; switch (id) { case MAVLink.MAV_DATA_STREAM.ALL: break; case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_SYS_STATUS]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.EXTRA1: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_ATTITUDE]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.EXTRA2: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_VFR_HUD]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.EXTRA3: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_AHRS]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.POSITION: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.RAW_SENSORS: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RAW_IMU]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.RC_CHANNELS: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW]; if (hzratecheck(pps, hzrate)) { return; } break; } //packetspersecond[temp[5]]; if (pps == 0 && hzrate == 0) { return; } //log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate); await GetDatastreamAsync(id, hzrate); } public async Task StopDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) { double pps = 0; switch (id) { case MAVLink.MAV_DATA_STREAM.ALL: break; case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_SYS_STATUS]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.EXTRA1: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_ATTITUDE]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.EXTRA2: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_VFR_HUD]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.EXTRA3: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_AHRS]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.POSITION: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED]; if (hzratecheck(pps, hzrate)) { return; } break; case MAVLink.MAV_DATA_STREAM.RAW_SENSORS: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RAW_IMU]; if (hzratecheck(pps, hzrate)) { return; } break; //case MAVLink.MAV_DATA_STREAM.SCALED_IMU2: // break; case MAVLink.MAV_DATA_STREAM.RC_CHANNELS: if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW]; if (hzratecheck(pps, hzrate)) { return; } break; } //log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate); await StopGetDatastreamAsync(id, hzrate); } bool hzratecheck(double pps, int hzrate) { if (hzrate == 0 && pps == 0) { return true; } else if (hzrate == 1 && pps >= 0.5 && pps <= 2) { return true; } else if (hzrate == 3 && pps >= 2 && hzrate < 5) { return true; } else if (hzrate == 10 && pps > 5 && hzrate < 15) { return true; } else if (hzrate > 15 && pps > 15) { return true; } return false; } async Task GetDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) { MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t(); req.target_system = sysid; req.target_component = compid; req.req_message_rate = hzrate; req.start_stop = 1; // start req.req_stream_id = (byte)id; // id // send each one twice. await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); } async Task StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) { MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t(); req.target_system = sysid; req.target_component = compid; req.req_message_rate = 0; req.start_stop = 0; // start req.req_stream_id = (byte)id; // id // send each one twice. await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); } public async Task SetParamAsync(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 = sysid, target_component = compid, param_type = (byte)param_types[paramname] }; byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname); Array.Resize(ref temp, 16); req.param_id = temp; req.param_value = (value); await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req); } public async Task 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 = sysid, target_component = compid, param_type = 4 }; byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname); Array.Resize(ref temp, 16); req.param_id = temp; req.param_value = (value); await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req); } public async Task SetModeAsync(ac2modes modein) { try { MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t(); if (TranslateMode(modein, ref mode)) { await SetModeAsync(mode); } } catch { RaiseMessageCreated("Failed to change Modes"); } } public async Task SetModeAsync(string modein) { try { MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t(); if (TranslateMode(modein, ref mode)) { await SetModeAsync(mode); } } catch { RaiseMessageCreated("Failed to change Modes"); } } public async Task SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0) { mode.base_mode |= (byte)base_mode; await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); await Task.Delay(10); await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); } public async Task InjectGpsDataAsync(byte[] data, ushort length) { MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t(); var msglen = 110; var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1; for (int a = 0; a < len; a++) { gps.data = new byte[msglen]; int copy = Math.Min(length - a * msglen, msglen); Array.Copy(data, a * msglen, gps.data, 0, copy); gps.len = (byte)copy; gps.target_component = compid; gps.target_system = sysid; await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps); } } public bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode) { //MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t(); mode.target_system = sysid; 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: RaiseMessageCreated("错误模式: " + modein); return false; } return true; } public bool TranslateMode(string modein, ref MAVLink.mavlink_set_mode_t mode) { //MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t(); mode.target_system = sysid; var modeValue = EnumTranslator.GetValue(modein.ToUpper()); switch (modeValue) { case (int)ac2modes.STABILIZE: case (int)ac2modes.AUTO: case (int)ac2modes.RTL: case (int)ac2modes.LOITER: case (int)ac2modes.ACRO: case (int)ac2modes.ALT_HOLD: case (int)ac2modes.CIRCLE: case (int)ac2modes.POSITION: case (int)ac2modes.LAND: case (int)ac2modes.OF_LOITER: case (int)ac2modes.GUIDED: mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED; mode.custom_mode = (uint)modeValue; break; default: RaiseMessageCreated("错误模式: " + modein); return false; } return true; } public async Task GetWPsAsync() { MAVLink.mavlink_mission_request_list_t req = new MAVLink.mavlink_mission_request_list_t(); req.target_system = sysid; req.target_component = compid; await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req); } /// /// Gets specfied WP /// /// /// WP public async Task GetWPAsync(ushort index) { giveComport = true; MAVLink.mavlink_mission_request_t req = new MAVLink.mavlink_mission_request_t(); req.target_system = sysid; req.target_component = compid; req.seq = index; //Console.WriteLine("getwp req "+ DateTime.Now.Millisecond); // request await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_REQUEST, req); } public async Task SetWPTotalAsync(ushort wp_total) { giveComport = true; MAVLink.mavlink_mission_count_t req = new MAVLink.mavlink_mission_count_t(); req.target_system = sysid; req.target_component = compid; // MSG_NAMES.MISSION_COUNT req.count = wp_total; await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_COUNT, req); } /// 任务信息。 /// 任务编号。 /// 坐标系。 /// 0：在 Auto 模式下飞往指定位置；2：切到 GUIDED 模式，立即飞往指定位置；3：仅改变高度，立即执行。 /// 指示是否自动继续执行下一个任务。 public async Task WriteWPAsync(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0, byte autocontinue = 1) { MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t(); req.target_system = sysid; req.target_component = compid; // MSG_NAMES.MISSION_ITEM req.command = loc.id; req.current = current; req.autocontinue = autocontinue; req.frame = (byte)frame; req.y = (float)(loc.lng); req.x = (float)(loc.lat); req.z = (float)(loc.alt); req.param1 = loc.p1; req.param2 = loc.p2; req.param3 = loc.p3; req.param4 = loc.p4; req.seq = index; //log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index); // request await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_ITEM, req); } const float rad2deg = (float)(180 / Math.PI); const float deg2rad = (float)(1.0 / rad2deg); public async Task SetSensorOffsetsAsync(sensoroffsetsenum sensor, float x, float y, float z) { return await DoCommandAsync(Plane.Protocols.MAVLink.MAV_CMD.PREFLIGHT_SET_SENSOR_OFFSETS, (int)sensor, x, y, z, 0, 0, 0); } public byte rssi; private void RaiseMessageCreated(string message) { var handler = MessageCreated; if (handler != null) { var e = new MessageCreatedEventArgs { Message = message }; RunOnUIThread(() => { handler(this, e); }); } } private void RunOnUIThread(Action action) { if (SynchronizationContext.Current == _uiSyncContext) { action(); } else { _uiSyncContext.Post(delegate { action(); }, null); } } private void RaiseEventOnUIThread(EventHandler ev) { if (ev != null) { RunOnUIThread(() => ev(this, EventArgs.Empty)); } } private void RaiseEventOnUIThread(EventHandler ev, TEventArgs e) { if (ev != null) { RunOnUIThread(() => ev(this, e)); } } } }