using System; using System.Collections.Generic; using System.Text; using System.IO.Ports; using System.Runtime.InteropServices; using System.Collections; // hashs using System.Diagnostics; // stopwatch using System.Reflection; using System.Reflection.Emit; using System.IO; namespace ArdupilotMega { public partial class MAVLink { public ICommsSerial BaseStream = new SerialPort(); /// /// used for outbound packet sending /// byte packetcount = 0; public byte sysid = 0; public byte compid = 0; public Hashtable param = new Hashtable(); public static byte[][] packets = new byte[256][]; public double[] packetspersecond = new double[256]; DateTime[] packetspersecondbuild = new DateTime[256]; static object objlock = new object(); static object readlock = new object(); object logwritelock = new object(); public DateTime lastvalidpacket = DateTime.Now; bool oldlogformat = false; byte mavlinkversion = 0; public PointLatLngAlt[] wps = new PointLatLngAlt[200]; public bool debugmavlink = false; public bool logreadmode = false; public DateTime lastlogread = DateTime.MinValue; public BinaryReader logplaybackfile = null; public BinaryWriter logfile = null; public static byte[] streams = new byte[256]; int bps1 = 0; int bps2 = 0; public int bps = 0; public DateTime bpstime = DateTime.Now; int recvpacketcount = 0; float synclost; float packetslost = 0; float packetsnotlost = 0; DateTime packetlosttimer = DateTime.Now; //Stopwatch stopwatch = new Stopwatch(); public void Close() { BaseStream.Close(); } public void Open() { Open(false); } public void Open(bool getparams) { if (BaseStream.IsOpen) return; System.Windows.Forms.Form frm = Common.LoadingBox("Mavlink Connecting..", "Mavlink Connecting.."); frm.TopMost = true; // reset sysid = 0; compid = 0; param = new Hashtable(); try { MainV2.givecomport = true; BaseStream.ReadBufferSize = 4 * 1024; lock (objlock) // so we dont have random traffic { BaseStream.Open(); BaseStream.DiscardInBuffer(); System.Threading.Thread.Sleep(200); // allow reset to work if (BaseStream.DtrEnable) BaseStream.DtrEnable = false; // allow 2560 connect timeout on usb System.Threading.Thread.Sleep(1000); } byte[] buffer; byte[] buffer1; DateTime start = DateTime.Now; int count = 0; while (true) { System.Windows.Forms.Application.DoEvents(); // incase we are in setup mode BaseStream.WriteLine("planner\rgcs\r"); frm.Controls[0].Text = (start.AddSeconds(30) - DateTime.Now).Seconds.ToString("Timeout in 0"); if (lastbad[0] == '!' && lastbad[1] == 'G' || lastbad[0] == 'G' && lastbad[1] == '!') // waiting for gps lock { frm.Controls[0].Text = "Waiting for GPS detection.."; start = start.AddSeconds(5); // each round is 1.1 seconds } System.Windows.Forms.Application.DoEvents(); if (!(start.AddSeconds(30) > DateTime.Now)) { /* System.Windows.Forms.DialogResult dr = System.Windows.Forms.MessageBox.Show("Data recevied but no mavlink packets where read from this port\nWhat do you want to do", "Read Fail", System.Windows.Forms.MessageBoxButtons.RetryCancel); if (dr == System.Windows.Forms.DialogResult.Retry) { port.toggleDTRnow(); // force reset on usb start = DateTime.Now; } else*/ { frm.Close(); this.Close(); throw new Exception("No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nIt might also be waiting for GPS Lock\nAPM Planner waits for 2 valid heartbeat packets before connecting"); } } System.Threading.Thread.Sleep(1); // incase we are in setup mode BaseStream.WriteLine("planner\rgcs\r"); System.Windows.Forms.Application.DoEvents(); buffer = getHeartBeat(); System.Windows.Forms.Application.DoEvents(); // incase we are in setup mode BaseStream.WriteLine("planner\rgcs\r"); System.Threading.Thread.Sleep(1); System.Windows.Forms.Application.DoEvents(); buffer1 = getHeartBeat(); System.Windows.Forms.Application.DoEvents(); try { Console.WriteLine("MAv Data: len " + buffer.Length + " btr " + BaseStream.BytesToRead); } catch { } count++; if (buffer.Length > 5 && buffer1.Length > 5 && buffer[3] == buffer1[3] && buffer[4] == buffer1[4]) { __mavlink_heartbeat_t hb = new __mavlink_heartbeat_t(); object temp = hb; MAVLink.ByteArrayToStructure(buffer, ref temp, 6); hb = (MAVLink.__mavlink_heartbeat_t)(temp); mavlinkversion = hb.mavlink_version; sysid = buffer[3]; compid = buffer[4]; recvpacketcount = buffer[2]; Console.WriteLine("ID sys " + sysid + " comp " + compid + " ver" + mavlinkversion); break; } } frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + " compid "+compid+") "; frm.Refresh(); if (getparams == true) getParamList(); } catch (Exception e) { MainV2.givecomport = false; frm.Close(); throw e; } frm.Close(); MainV2.givecomport = false; Console.WriteLine("Done open " + sysid + " " + compid); packetslost = 0; } public static byte[] StructureToByteArrayEndian(params object[] list) { // The copy is made becuase SetValue won't work on a struct. // Boxing was used because SetValue works on classes/objects. // Unfortunately, it results in 2 copy operations. object thisBoxed = list[0]; // Why make a copy? Type test = thisBoxed.GetType(); int offset = 0; byte[] data = new byte[Marshal.SizeOf(thisBoxed)]; // System.Net.IPAddress.NetworkToHostOrder is used to perform byte swapping. // To convert unsigned to signed, 'unchecked()' was used. // See http://stackoverflow.com/questions/1131843/how-do-i-convert-uint-to-int-in-c object fieldValue; TypeCode typeCode; byte[] temp; // Enumerate each structure field using reflection. foreach (var field in test.GetFields()) { // field.Name has the field's name. fieldValue = field.GetValue(thisBoxed); // Get value // Get the TypeCode enumeration. Multiple types get mapped to a common typecode. typeCode = Type.GetTypeCode(fieldValue.GetType()); switch (typeCode) { case TypeCode.Single: // float { temp = BitConverter.GetBytes((Single)fieldValue); Array.Reverse(temp); Array.Copy(temp, 0, data, offset, sizeof(Single)); break; } case TypeCode.Int32: { temp = BitConverter.GetBytes((Int32)fieldValue); Array.Reverse(temp); Array.Copy(temp, 0, data, offset, sizeof(Int32)); break; } case TypeCode.UInt32: { temp = BitConverter.GetBytes((UInt32)fieldValue); Array.Reverse(temp); Array.Copy(temp, 0, data, offset, sizeof(UInt32)); break; } case TypeCode.Int16: { temp = BitConverter.GetBytes((Int16)fieldValue); Array.Reverse(temp); Array.Copy(temp, 0, data, offset, sizeof(Int16)); break; } case TypeCode.UInt16: { temp = BitConverter.GetBytes((UInt16)fieldValue); Array.Reverse(temp); Array.Copy(temp, 0, data, offset, sizeof(UInt16)); break; } case TypeCode.Int64: { temp = BitConverter.GetBytes((Int64)fieldValue); Array.Reverse(temp); Array.Copy(temp, 0, data, offset, sizeof(Int64)); break; } case TypeCode.UInt64: { temp = BitConverter.GetBytes((UInt64)fieldValue); Array.Reverse(temp); Array.Copy(temp, 0, data, offset, sizeof(UInt64)); break; } case TypeCode.Double: { temp = BitConverter.GetBytes((Double)fieldValue); Array.Reverse(temp); Array.Copy(temp, 0, data, offset, sizeof(Double)); break; } case TypeCode.Byte: { data[offset] = (Byte)fieldValue; break; } default: { //System.Diagnostics.Debug.Fail("No conversion provided for this type : " + typeCode.ToString()); break; } }; // switch if (typeCode == TypeCode.Object) { int length = ((byte[])fieldValue).Length; Array.Copy(((byte[])fieldValue), 0, data, offset, length); offset += length; } else { offset += Marshal.SizeOf(fieldValue); } } // foreach return data; } // Swap byte[] getHeartBeat() { DateTime start = DateTime.Now; while (true) { byte[] buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_HEARTBEAT) { return buffer; } } if (DateTime.Now > start.AddMilliseconds(2200)) // was 1200 , now 2.2 sec return new byte[0]; } } /// /// Generate a Mavlink Packet and write to serial /// /// type number /// struct of data public void generatePacket(byte messageType, object indata) { byte[] data; if (mavlinkversion == 3) { data = StructureToByteArray(indata); } else { data = StructureToByteArrayEndian(indata); } //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); byte[] packet = new byte[data.Length + 6 + 2]; if (mavlinkversion == 3) { packet[0] = 254; } else if (mavlinkversion == 2) { packet[0] = (byte)'U'; } packet[1] = (byte)data.Length; packet[2] = packetcount; packet[3] = 255; // this is always 255 - MYGCS packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_WAYPOINTPLANNER; packet[5] = messageType; int i = 6; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = crc_calculate(packet, packet[1] + 6); if (mavlinkversion == 3) { checksum = crc_accumulate(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 (BaseStream.IsOpen) { lock (objlock) { BaseStream.Write(packet, 0, i); } } try { if (logfile != null) { lock (logwritelock) { byte[] datearray = BitConverter.GetBytes((UInt64)((DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds * 1000)); //ASCIIEncoding.ASCII.GetBytes(DateTime.Now.ToBinary() + ":"); Array.Reverse(datearray); logfile.Write(datearray, 0, datearray.Length); logfile.Write(packet, 0, i); } } } catch { } if (messageType == ArdupilotMega.MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM) { try { BinaryWriter bw = new BinaryWriter(File.OpenWrite("serialsent.raw")); bw.Seek(0, SeekOrigin.End); bw.Write(packet, 0, i); bw.Write((byte)'\n'); bw.Close(); } catch { } // been getting errors from this. people must have it open twice. } packetcount++; //System.Threading.Thread.Sleep(1); } public bool Write(string line) { lock (objlock) { BaseStream.Write(line); } return true; } /// /// Set parameter on apm /// /// name as a string /// public bool setParam(string paramname, float value) { MainV2.givecomport = true; __mavlink_param_set_t req = new __mavlink_param_set_t(); req.target_system = sysid; req.target_component = compid; byte[] temp = ASCIIEncoding.ASCII.GetBytes(paramname); modifyParamForDisplay(false, paramname, ref value); Array.Resize(ref temp, 15); req.param_id = temp; req.param_value = (value); generatePacket(MAVLINK_MSG_ID_PARAM_SET, req); Console.WriteLine("setParam '{0}' = '{1}' sysid {2} compid {3}", paramname, req.param_value, sysid, compid); DateTime start = DateTime.Now; int retrys = 3; while (true) { if (!(start.AddMilliseconds(500) > DateTime.Now)) { if (retrys > 0) { Console.WriteLine("setParam Retry " + retrys); generatePacket(MAVLINK_MSG_ID_PARAM_SET, req); start = DateTime.Now; retrys--; continue; } MainV2.givecomport = false; throw new Exception("Timeout on read - setParam " + paramname); } byte[] buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE) { __mavlink_param_value_t par = new __mavlink_param_value_t(); object tempobj = par; ByteArrayToStructure(buffer, ref tempobj, 6); par = (__mavlink_param_value_t)tempobj; string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id); int pos = st.IndexOf('\0'); if (pos != -1) { st = st.Substring(0, pos); } if (st != paramname) { Console.WriteLine("MAVLINK bad param responce - {0} vs {1}",paramname,st); continue; } param[st] = (par.param_value); MainV2.givecomport = false; //System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte return true; } } } } public struct _param { public string name; public float value; } /// /// Get param list from apm /// /// public Hashtable getParamList() { MainV2.givecomport = true; List missed = new List(); _param[] paramarray = new _param[300]; __mavlink_param_request_list_t req = new __mavlink_param_request_list_t(); req.target_system = sysid; req.target_component = compid; generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req); DateTime start = DateTime.Now; int retrys = 3; int nextid = 0; int a = 0; int z = 5; while (a < z) { if (!(start.AddMilliseconds(5000) > DateTime.Now)) { if (retrys > 0) { Console.WriteLine("getParamList Retry " + retrys + " sys " + sysid + " comp " + compid); generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req); start = DateTime.Now; retrys--; continue; } MainV2.givecomport = false; throw new Exception("Timeout on read - getParamList"); } byte[] buffer = readPacket(); if (buffer.Length > 5) { //stopwatch.Reset(); //stopwatch.Start(); if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE) { start = DateTime.Now; __mavlink_param_value_t par = new __mavlink_param_value_t(); object temp = par; ByteArrayToStructure(buffer, ref temp, 6); par = (__mavlink_param_value_t)temp; z = (par.param_count); Console.WriteLine(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (z - 1)); if (nextid == (par.param_index)) { nextid++; } else { if (retrys > 0) { generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req); a = 0; nextid = 0; retrys--; continue; } missed.Add(nextid); // for later devel MainV2.givecomport = false; throw new Exception("Missed ID expecting " + nextid + " got " + (par.param_index) + "\nPlease try loading again"); } string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id); int pos = st.IndexOf('\0'); if (pos != -1) { st = st.Substring(0, pos); } modifyParamForDisplay(true, st, ref par.param_value); param[st] = (par.param_value); a++; } else { //Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " " + this.BytesToRead); } //stopwatch.Stop(); //Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed); } } MainV2.givecomport = false; return param; } public static void modifyParamForDisplay(bool fromapm, string paramname, ref float value) { if (paramname.ToUpper().EndsWith("_IMAX") || paramname.ToUpper().EndsWith("ALT_HOLD_RTL") || paramname.ToUpper().EndsWith("TRIM_ARSPD_CM") || paramname.ToUpper().EndsWith("XTRK_ANGLE_CD") || paramname.ToUpper().EndsWith("LIM_PITCH_MAX") || paramname.ToUpper().EndsWith("LIM_PITCH_MIN") || paramname.ToUpper().EndsWith("LIM_ROLL_CD") || paramname.ToUpper().EndsWith("PITCH_MAX") || paramname.ToUpper().EndsWith("WP_SPEED_MAX")) { if (paramname.ToUpper().EndsWith("THR_HOLD_IMAX")) { return; } if (fromapm) { value /= 100.0f; } else { value *= 100.0f; } } } /// /// Stops all requested data packets. /// public void stopall(bool forget) { __mavlink_request_data_stream_t req = new __mavlink_request_data_stream_t(); req.target_system = sysid; req.target_component = compid; req.req_message_rate = 10; req.start_stop = 0; // stop req.req_stream_id = 0; // all // reset all if (forget) { lock (objlock) { streams = new byte[streams.Length]; } } // no error on bad try { generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); System.Threading.Thread.Sleep(20); generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); System.Threading.Thread.Sleep(20); generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); Console.WriteLine("Stopall Done"); } catch { } } public void setWPACK() { MAVLink.__mavlink_waypoint_ack_t req = new MAVLink.__mavlink_waypoint_ack_t(); req.target_system = sysid; req.target_component = compid; req.type = 0; generatePacket(MAVLINK_MSG_ID_WAYPOINT_ACK, req); } public bool setWPCurrent(ushort index) { MainV2.givecomport = true; byte[] buffer; __mavlink_waypoint_set_current_t req = new __mavlink_waypoint_set_current_t(); req.target_system = sysid; req.target_component = compid; req.seq = index; generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req); DateTime start = DateTime.Now; int retrys = 5; while (true) { if (!(start.AddMilliseconds(2000) > DateTime.Now)) { if (retrys > 0) { Console.WriteLine("setWPCurrent Retry " + retrys); generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req); start = DateTime.Now; retrys--; continue; } MainV2.givecomport = false; throw new Exception("Timeout on read - setWPCurrent"); } buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_CURRENT) { MainV2.givecomport = false; return true; } } } } public bool doAction(MAV_ACTION actionid) { MainV2.givecomport = true; byte[] buffer; __mavlink_action_t req = new __mavlink_action_t(); req.target = sysid; req.target_component = compid; req.action = (byte)actionid; generatePacket(MAVLINK_MSG_ID_ACTION, req); DateTime start = DateTime.Now; int retrys = 3; int timeout = 2000; // imu calib take a little while if (actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_ACC || actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_GYRO || actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_MAG || actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_PRESSURE || actionid == MAV_ACTION.MAV_ACTION_REBOOT) { retrys = 1; timeout = 6000; } while (true) { if (!(start.AddMilliseconds(timeout) > DateTime.Now)) { if (retrys > 0) { Console.WriteLine("doAction Retry " + retrys); generatePacket(MAVLINK_MSG_ID_ACTION, req); start = DateTime.Now; retrys--; continue; } MainV2.givecomport = false; throw new Exception("Timeout on read - doAction"); } buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_ACTION_ACK) { if (buffer[7] == 1) { MainV2.givecomport = false; return true; } else { MainV2.givecomport = false; return false; } } } } } public void requestDatastream(byte id, byte hzrate) { lock (objlock) { streams[id] = hzrate; } double pps = 0; switch (id) { case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_ALL: break; case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_EXTENDED_STATUS: if (packetspersecondbuild[MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLINK_MSG_ID_SYS_STATUS]; if (hzratecheck(pps, hzrate)) { return; } break; case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_EXTRA1: if (packetspersecondbuild[MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLINK_MSG_ID_ATTITUDE]; if (hzratecheck(pps, hzrate)) { return; } break; case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_EXTRA2: if (packetspersecondbuild[MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLINK_MSG_ID_VFR_HUD]; if (hzratecheck(pps, hzrate)) { return; } break; case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_EXTRA3: break; case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_POSITION: if (packetspersecondbuild[MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLINK_MSG_ID_GLOBAL_POSITION_INT]; if (hzratecheck(pps, hzrate)) { return; } break; case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RAW_CONTROLLER: if (packetspersecondbuild[MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLINK_MSG_ID_RC_CHANNELS_SCALED]; if (hzratecheck(pps, hzrate)) { return; } break; case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RAW_SENSORS: if (packetspersecondbuild[MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLINK_MSG_ID_RAW_IMU]; if (hzratecheck(pps, hzrate)) { return; } break; case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RC_CHANNELS: if (packetspersecondbuild[MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLINK_MSG_ID_RC_CHANNELS_RAW]; if (hzratecheck(pps, hzrate)) { return; } break; } //packetspersecond[temp[5]]; if (pps == 0 && hzrate == 0) { return; } Console.WriteLine("Request stream {0} at {1} hz : currently {2}", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate, pps); getDatastream(id, hzrate); } // returns true for ok 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; } void getDatastream(byte id, byte hzrate) { __mavlink_request_data_stream_t req = new __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 = id; // id generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); } /// /// Returns WP count /// /// public byte getWPCount() { MainV2.givecomport = true; byte[] buffer; __mavlink_waypoint_request_list_t req = new __mavlink_waypoint_request_list_t(); req.target_system = sysid; req.target_component = compid; // request list generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req); DateTime start = DateTime.Now; int retrys = 6; while (true) { if (!(start.AddMilliseconds(500) > DateTime.Now)) { if (retrys > 0) { Console.WriteLine("getWPCount Retry " + retrys + " - giv com " + MainV2.givecomport); generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req); start = DateTime.Now; retrys--; continue; } MainV2.givecomport = false; //return (byte)int.Parse(param["WP_TOTAL"].ToString()); throw new Exception("Timeout on read - getWPCount"); } buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT) { Console.WriteLine("wpcount: " + buffer[9]); MainV2.givecomport = false; return buffer[9]; // should be ushort, but apm has limited wp count < byte } else { Console.WriteLine(DateTime.Now + " PC wpcount " + buffer[5] + " need " + MAVLINK_MSG_ID_WAYPOINT_COUNT + " " + this.BaseStream.BytesToRead); } } } } /// /// Gets specfied WP /// /// /// WP public Locationwp getWP(ushort index) { MainV2.givecomport = true; Locationwp loc = new Locationwp(); __mavlink_waypoint_request_t req = new __mavlink_waypoint_request_t(); req.target_system = sysid; req.target_component = compid; req.seq = index; //Console.WriteLine("getwp req "+ DateTime.Now.Millisecond); // request generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req); DateTime start = DateTime.Now; int retrys = 5; while (true) { if (!(start.AddMilliseconds(800) > DateTime.Now)) // apm times out after 1000ms { if (retrys > 0) { Console.WriteLine("getWP Retry " + retrys); generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req); start = DateTime.Now; retrys--; continue; } MainV2.givecomport = false; throw new Exception("Timeout on read - getWP"); } //Console.WriteLine("getwp read " + DateTime.Now.Millisecond); byte[] buffer = readPacket(); //Console.WriteLine("getwp readend " + DateTime.Now.Millisecond); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT) { //Console.WriteLine("getwp ans " + DateTime.Now.Millisecond); __mavlink_waypoint_t wp = new __mavlink_waypoint_t(); object temp = (object)wp; //Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6); ByteArrayToStructure(buffer, ref temp, 6); wp = (__mavlink_waypoint_t)(temp); loc.options = (byte)(wp.frame & 0x1); loc.id = (byte)(wp.command); loc.p1 = (byte)(wp.param1); if (loc.id < (byte)MAV_CMD.LAST) { loc.alt = (int)((wp.z) * 100); loc.lat = (int)((wp.x) * 10000000); loc.lng = (int)((wp.y) * 10000000); } else { loc.alt = (int)((wp.z)); loc.lat = (int)((wp.x)); loc.lng = (int)((wp.y)); switch (loc.id) { // Switch to map APM command fields inot MAVLink command fields case (byte)MAV_CMD.LOITER_TURNS: case (byte)MAV_CMD.TAKEOFF: case (byte)MAV_CMD.DO_SET_HOME: case (byte)MAV_CMD.DO_SET_ROI: loc.alt = (int)((wp.z) * 100); loc.lat = (int)((wp.x) * 10000000); loc.lng = (int)((wp.y) * 10000000); loc.p1 = (byte)wp.param1; break; case (byte)MAV_CMD.CONDITION_CHANGE_ALT: loc.lat = (int)wp.param1; loc.p1 = 0; break; case (byte)MAV_CMD.LOITER_TIME: if (MainV2.APMFirmware == MainV2.Firmwares.ArduPlane) { loc.p1 = (byte)(wp.param1 / 10); // APM loiter time is in ten second increments } else { loc.p1 = (byte)wp.param1; } break; case (byte)MAV_CMD.CONDITION_DELAY: case (byte)MAV_CMD.CONDITION_DISTANCE: loc.lat = (int)wp.param1; break; case (byte)MAV_CMD.DO_JUMP: loc.lat = (int)wp.param2; loc.p1 = (byte)wp.param1; break; case (byte)MAV_CMD.DO_REPEAT_SERVO: loc.lng = (int)wp.param4; goto case (byte)MAV_CMD.DO_CHANGE_SPEED; case (byte)MAV_CMD.DO_REPEAT_RELAY: case (byte)MAV_CMD.DO_CHANGE_SPEED: loc.lat = (int)wp.param3; loc.alt = (int)wp.param2; loc.p1 = (byte)wp.param1; break; case (byte)MAV_CMD.DO_SET_PARAMETER: case (byte)MAV_CMD.DO_SET_RELAY: case (byte)MAV_CMD.DO_SET_SERVO: loc.alt = (int)wp.param2; loc.p1 = (byte)wp.param1; break; case (byte)MAV_CMD.WAYPOINT: loc.p1 = (byte)wp.param1; break; } } Console.WriteLine("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng, loc.options); break; } else { Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); } } } MainV2.givecomport = false; return loc; } public object DebugPacket(byte[] datin) { string text = ""; return DebugPacket(datin, ref text); } /// /// Print entire decoded packet to console /// /// packet byte array /// struct of data public object DebugPacket(byte[] datin, ref string text) { string textoutput; try { if (datin.Length > 5) { byte header = datin[0]; byte length = datin[1]; byte seq = datin[2]; byte sysid = datin[3]; byte compid = datin[4]; byte messid = datin[5]; textoutput = string.Format("{0:X} {1:X} {2:X} {3:X} {4:X} {5:X} ", header, length, seq, sysid, compid, messid); object data = Activator.CreateInstance(mavstructs[messid]); ByteArrayToStructure(datin, ref data, 6); Type test = data.GetType(); textoutput = textoutput + test.Name + " "; foreach (var field in test.GetFields()) { // field.Name has the field's name. object fieldValue = field.GetValue(data); // Get value if (field.FieldType.IsArray) { textoutput = textoutput + field.Name + "="; byte[] crap = (byte[])fieldValue; foreach (byte fiel in crap) { textoutput = textoutput + fiel + ","; } textoutput = textoutput + " "; } else { textoutput = textoutput + field.Name + "=" + fieldValue.ToString() + " "; } } textoutput = textoutput + " Len:" + datin.Length + "\r\n"; Console.Write(textoutput); if (text != null) text = textoutput; return data; } } catch { } return null; } /// /// Sets wp total count /// /// public void setWPTotal(ushort wp_total) { MainV2.givecomport = true; __mavlink_waypoint_count_t req = new __mavlink_waypoint_count_t(); req.target_system = sysid; req.target_component = compid; // MAVLINK_MSG_ID_WAYPOINT_COUNT req.count = wp_total; generatePacket(MAVLINK_MSG_ID_WAYPOINT_COUNT, req); DateTime start = DateTime.Now; int retrys = 3; while (true) { if (!(start.AddMilliseconds(700) > DateTime.Now)) { if (retrys > 0) { Console.WriteLine("setWPTotal Retry " + retrys); generatePacket(MAVLINK_MSG_ID_WAYPOINT_COUNT, req); start = DateTime.Now; retrys--; continue; } MainV2.givecomport = false; throw new Exception("Timeout on read - setWPTotal"); } byte[] buffer = readPacket(); if (buffer.Length > 9) { if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST && buffer[9] == 0) { param["WP_TOTAL"] = (float)wp_total - 1; MainV2.givecomport = false; return; } else { //Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); } } } } /// /// Save wp to eeprom /// /// location struct /// wp no /// global or relative /// 0 = no , 2 = guided mode public void setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current) { MainV2.givecomport = true; __mavlink_waypoint_t req = new __mavlink_waypoint_t(); req.target_system = sysid; req.target_component = compid; // MAVLINK_MSG_ID_WAYPOINT req.command = loc.id; req.param1 = loc.p1; req.current = current; if (loc.id < (byte)MAV_CMD.LAST) { req.frame = (byte)frame; req.y = (float)(loc.lng / 10000000.0); req.x = (float)(loc.lat / 10000000.0); req.z = (float)(loc.alt / 100.0); } else { req.frame = (byte)MAVLink.MAV_FRAME.MAV_FRAME_MISSION; // mission req.y = (float)(loc.lng); req.x = (float)(loc.lat); req.z = (float)(loc.alt); switch (loc.id) { // Switch to map APM command fields inot MAVLink command fields case (byte)MAV_CMD.LOITER_TURNS: case (byte)MAV_CMD.TAKEOFF: req.param1 = loc.p1; break; case (byte)MAV_CMD.DO_SET_HOME: req.y = (float)(loc.lng / 10000000.0); req.x = (float)(loc.lat / 10000000.0); req.z = (float)(loc.alt / 100.0); req.param1 = loc.p1; break; case (byte)MAV_CMD.CONDITION_CHANGE_ALT: req.param1 = loc.lat; req.x = 0; req.y = 0; break; case (byte)MAV_CMD.LOITER_TIME: req.param1 = loc.p1 * 10; // APM loiter time is in ten second increments break; case (byte)MAV_CMD.CONDITION_DELAY: case (byte)MAV_CMD.CONDITION_DISTANCE: req.param1 = loc.lat; break; case (byte)MAV_CMD.DO_JUMP: req.param2 = loc.lat; req.param1 = loc.p1; break; case (byte)MAV_CMD.DO_REPEAT_SERVO: req.param4 = loc.lng; goto case (byte)MAV_CMD.DO_CHANGE_SPEED; case (byte)MAV_CMD.DO_REPEAT_RELAY: case (byte)MAV_CMD.DO_CHANGE_SPEED: req.param3 = loc.lat; req.param2 = loc.alt; req.param1 = loc.p1; break; case (byte)MAV_CMD.DO_SET_PARAMETER: case (byte)MAV_CMD.DO_SET_RELAY: case (byte)MAV_CMD.DO_SET_SERVO: req.param2 = loc.alt; req.param1 = loc.p1; break; } } req.seq = index; Console.WriteLine("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 generatePacket(MAVLINK_MSG_ID_WAYPOINT, req); DateTime start = DateTime.Now; int retrys = 6; while (true) { if (!(start.AddMilliseconds(500) > DateTime.Now)) { if (retrys > 0) { Console.WriteLine("setWP Retry " + retrys); generatePacket(MAVLINK_MSG_ID_WAYPOINT, req); start = DateTime.Now; retrys--; continue; } MainV2.givecomport = false; throw new Exception("Timeout on read - setWP"); } byte[] buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_ACK) { //__mavlink_waypoint_request_t Console.WriteLine("set wp " + index + " ACK 47 : " + buffer[5]); break; } else if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST) { __mavlink_waypoint_request_t ans = new __mavlink_waypoint_request_t(); object temp = (object)ans; //Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6); ByteArrayToStructure(buffer, ref temp, 6); ans = (__mavlink_waypoint_request_t)(temp); if (ans.seq == (index + 1)) { Console.WriteLine("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]); MainV2.givecomport = false; break; } else { Console.WriteLine("set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + buffer[5] + " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component); //break; } } else { //Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]); } } } } /// /// used for last bad serial characters /// byte[] lastbad = new byte[2]; /// /// Serial Reader to read mavlink packets. POLL method /// /// public byte[] readPacket() { byte[] temp = new byte[300]; int count = 0; int length = 0; int readcount = 0; lastbad = new byte[2]; BaseStream.ReadTimeout = 1100; // 1100 ms between bytes DateTime start = DateTime.Now; lock (readlock) { while (BaseStream.IsOpen || logreadmode) { try { if (readcount > 300) { Console.WriteLine("MAVLink readpacket No valid mavlink packets"); break; } readcount++; if (logreadmode) { try { if (logplaybackfile.BaseStream.Position == 0) { if (logplaybackfile.PeekChar() == '-') { oldlogformat = true; } else { oldlogformat = false; } } } catch { oldlogformat = false; } if (oldlogformat) { temp = readlogPacket(); //old style log } else { temp = readlogPacketMavlink(); } } else { MainV2.cs.datetime = DateTime.Now; temp[count] = (byte)BaseStream.ReadByte(); } } catch (Exception e) { Console.WriteLine("MAVLink readpacket read error: " + e.Message); break; } if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync { if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n') { Console.Write((char)temp[0]); } count = 0; lastbad[0] = lastbad[1]; lastbad[1] = temp[0]; temp[1] = 0; continue; } // reset count on valid packet readcount = 0; if (temp[0] == 'U' || temp[0] == 254) { length = temp[1] + 6 + 2 - 2; // data + header + checksum - U - length if (count >= 5 || logreadmode) { if (sysid != 0) { if (sysid != temp[3] || compid != temp[4]) { Console.WriteLine("Mavlink Bad Packet (not addressed to this MAV) got {0} {1} vs {2} {3}", temp[3], temp[4], sysid, compid); return new byte[0]; } } try { if (logreadmode) { } else { int to = 0; while (BaseStream.BytesToRead < (length - 4)) { if (to > 1000) { Console.WriteLine("MAVLINK: wait time out btr {0} len {1}", BaseStream.BytesToRead, length); break; } System.Threading.Thread.Sleep(1); System.Windows.Forms.Application.DoEvents(); // when connecting this is in the main thread to++; //Console.WriteLine("data " + 0 + " " + length + " aval " + BaseStream.BytesToRead); } int read = BaseStream.Read(temp, 6, length - 4); } //Console.WriteLine("data " + read + " " + length + " aval " + this.BytesToRead); count = length + 2; } catch { break; } break; } } count++; if (count == 299) break; } }// end readlock Array.Resize(ref temp, count); if (packetlosttimer.AddSeconds(10) < DateTime.Now) { packetlosttimer = DateTime.Now; packetslost = (int)(packetslost * 0.8f); packetsnotlost = (int)(packetsnotlost * 0.8f); } MainV2.cs.linkqualitygcs = (ushort)((packetsnotlost / (packetsnotlost + packetslost)) * 100); if (bpstime.Second != DateTime.Now.Second && !logreadmode) { Console.WriteLine("bps {0} loss {1} left {2}", bps1, synclost, BaseStream.BytesToRead); bps2 = bps1; // prev sec bps1 = 0; // current sec bpstime = DateTime.Now; } bps1 += temp.Length; bps = (bps1 + bps2) / 2; if (temp.Length >= 5 && temp[3] == 255 && logreadmode) // gcs packet { return temp;// new byte[0]; } ushort crc = crc_calculate(temp, temp.Length - 2); if (temp.Length > 5 && temp[0] == 254) { crc = crc_accumulate(MAVLINK_MESSAGE_CRCS[temp[5]], crc); } if (temp.Length < 5 || temp[temp.Length - 1] != (crc >> 8) || temp[temp.Length - 2] != (crc & 0xff)) { int packetno = 0; if (temp.Length > 5) { packetno = temp[5]; } Console.WriteLine("Mavlink Bad Packet (crc fail) len {0} crc {1} pkno {2}", temp.Length, crc, packetno); return new byte[0]; } try { if ((temp[0] == 'U' || temp[0] == 254) && temp.Length >= temp[1]) { if (temp[2] != ((recvpacketcount + 1) % 0x100)) { synclost++; // actualy sync loss's if (temp[2] < ((recvpacketcount + 1) % 0x100)) { packetslost += 0x100 - recvpacketcount + temp[2]; } else { packetslost += temp[2] - recvpacketcount; } Console.WriteLine("lost {0} pkts {1}", temp[2], (int)packetslost); } packetsnotlost++; recvpacketcount = temp[2]; //MAVLINK_MSG_ID_GPS_STATUS //if (temp[5] == MAVLINK_MSG_ID_GPS_STATUS) // Console.Write(temp[5] + " " + DateTime.Now.Millisecond + " " + packetspersecond[temp[5]] + " " + (DateTime.Now - packetspersecondbuild[temp[5]]).TotalMilliseconds + " \n"); if (double.IsInfinity(packetspersecond[temp[5]])) packetspersecond[temp[5]] = 0; packetspersecond[temp[5]] = (((1000 / ((DateTime.Now - packetspersecondbuild[temp[5]]).TotalMilliseconds) + packetspersecond[temp[5]]) / 2)); packetspersecondbuild[temp[5]] = DateTime.Now; //Console.WriteLine("Packet {0}",temp[5]); // store packet history lock (objlock) { packets[temp[5]] = temp; } if (debugmavlink) DebugPacket(temp); if (temp[5] == MAVLink.MAVLINK_MSG_ID_STATUSTEXT) // status text { string logdata = DateTime.Now.Millisecond + " " + Encoding.ASCII.GetString(temp, 6, temp.Length - 6); int ind = logdata.IndexOf('\0'); if (ind != -1) logdata = logdata.Substring(0, ind); Console.WriteLine(logdata); } if (temp[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT) { // clear old wps = new PointLatLngAlt[wps.Length]; } if (temp[5] == MAVLink.MAVLINK_MSG_ID_WAYPOINT) { __mavlink_waypoint_t wp = new __mavlink_waypoint_t(); object structtemp = (object)wp; //Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6); ByteArrayToStructure(temp, ref structtemp, 6); wp = (__mavlink_waypoint_t)(structtemp); wps[wp.seq] = new PointLatLngAlt(wp.x,wp.y,wp.z,wp.seq.ToString()); } try { if (logfile != null) { lock (logwritelock) { byte[] datearray = BitConverter.GetBytes((UInt64)((DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds * 1000)); //ASCIIEncoding.ASCII.GetBytes(DateTime.Now.ToBinary() + ":"); Array.Reverse(datearray); logfile.Write(datearray, 0, datearray.Length); logfile.Write(temp, 0, temp.Length); } } } catch { } } } catch { } lastvalidpacket = DateTime.Now; // Console.Write((DateTime.Now - start).TotalMilliseconds.ToString("00.000") + "\t" + temp.Length + " \r"); return temp; } byte[] readlogPacket() { byte[] temp = new byte[300]; sysid = 0; int a = 0; while (a < temp.Length && logplaybackfile.BaseStream.Position != logplaybackfile.BaseStream.Length) { temp[a] = (byte)logplaybackfile.BaseStream.ReadByte(); //Console.Write((char)temp[a]); if (temp[a] == ':') { break; } a++; if (temp[0] != '-') { a = 0; } } //Console.Write('\n'); //Encoding.ASCII.GetString(temp, 0, a); string datestring = Encoding.ASCII.GetString(temp, 0, a); //Console.WriteLine(datestring); long date = Int64.Parse(datestring); DateTime date1 = DateTime.FromBinary(date); lastlogread = date1; int length = 5; a = 0; while (a < length) { temp[a] = (byte)logplaybackfile.BaseStream.ReadByte(); if (a == 1) { length = temp[1] + 6 + 2 + 1; } a++; } return temp; } byte[] readlogPacketMavlink() { byte[] temp = new byte[300]; sysid = 0; //byte[] datearray = BitConverter.GetBytes((ulong)(DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds); byte[] datearray = new byte[8]; logplaybackfile.BaseStream.Read(datearray, 0, datearray.Length); Array.Reverse(datearray); DateTime date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc); UInt64 dateint = BitConverter.ToUInt64(datearray, 0); date1 = date1.AddMilliseconds(dateint / 1000); lastlogread = date1; MainV2.cs.datetime = lastlogread; int length = 5; int a = 0; while (a < length) { temp[a] = (byte)logplaybackfile.ReadByte(); if (temp[0] != 'U') { Console.WriteLine("lost sync byte {0} pos {1}", temp[0], logplaybackfile.BaseStream.Position); a = 0; continue; } if (a == 1) { length = temp[1] + 6 + 2; // 6 header + 2 checksum } a++; } return temp; } const int X25_INIT_CRC = 0xffff; const int X25_VALIDATE_CRC = 0xf0b8; ushort crc_accumulate(byte b, ushort crc) { unchecked { byte ch = (byte)(b ^ (byte)(crc & 0x00ff)); ch = (byte)(ch ^ (ch << 4)); return (ushort)((crc >> 8) ^ (ch << 8) ^ (ch << 3) ^ (ch >> 4)); } } ushort crc_calculate(byte[] pBuffer, int length) { if (length < 1) { return 0xffff; } // For a "message" of length bytes contained in the unsigned char array // pointed to by pBuffer, calculate the CRC // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed ushort crcTmp; int i; crcTmp = X25_INIT_CRC; for (i = 1; i < length; i++) // skips header U { crcTmp = crc_accumulate(pBuffer[i], crcTmp); //Console.WriteLine(crcTmp + " " + pBuffer[i] + " " + length); } return (crcTmp); } public static byte[] StructureToByteArray(object obj) { int len = Marshal.SizeOf(obj); byte[] arr = new byte[len]; IntPtr ptr = Marshal.AllocHGlobal(len); Marshal.StructureToPtr(obj, ptr, true); Marshal.Copy(ptr, arr, 0, len); Marshal.FreeHGlobal(ptr); return arr; } public static void ByteArrayToStructure(byte[] bytearray, ref object obj, int startoffset) { if (bytearray[0] == 'U') { ByteArrayToStructureEndian(bytearray, ref obj, startoffset); } else { int len = Marshal.SizeOf(obj); IntPtr i = Marshal.AllocHGlobal(len); // create structure from ptr obj = Marshal.PtrToStructure(i, obj.GetType()); try { // copy byte array to ptr Marshal.Copy(bytearray, startoffset, i, len); } catch (Exception ex) { Console.WriteLine("ByteArrayToStructure FAIL: error " + ex.ToString()); } obj = Marshal.PtrToStructure(i, obj.GetType()); Marshal.FreeHGlobal(i); } } public static void ByteArrayToStructureEndian(byte[] bytearray, ref object obj, int startoffset) { int len = Marshal.SizeOf(obj); IntPtr i = Marshal.AllocHGlobal(len); byte[] temparray = (byte[])bytearray.Clone(); // create structure from ptr obj = Marshal.PtrToStructure(i, obj.GetType()); // do endian swap object thisBoxed = obj; Type test = thisBoxed.GetType(); int reversestartoffset = startoffset; // Enumerate each structure field using reflection. foreach (var field in test.GetFields()) { // field.Name has the field's name. object fieldValue = field.GetValue(thisBoxed); // Get value // Get the TypeCode enumeration. Multiple types get mapped to a common typecode. TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType()); if (typeCode != TypeCode.Object) { Array.Reverse(temparray, reversestartoffset, Marshal.SizeOf(fieldValue)); reversestartoffset += Marshal.SizeOf(fieldValue); } else { reversestartoffset += ((byte[])fieldValue).Length; } } try { // copy byte array to ptr Marshal.Copy(temparray, startoffset, i, len); } catch (Exception ex) { Console.WriteLine("ByteArrayToStructure FAIL: error " + ex.ToString()); } obj = Marshal.PtrToStructure(i, obj.GetType()); Marshal.FreeHGlobal(i); } public short swapend11(short value) { int len = Marshal.SizeOf(value); byte[] temp = BitConverter.GetBytes(value); Array.Reverse(temp); return BitConverter.ToInt16(temp, 0); } public ushort swapend11(ushort value) { int len = Marshal.SizeOf(value); byte[] temp = BitConverter.GetBytes(value); Array.Reverse(temp); return BitConverter.ToUInt16(temp, 0); } public ulong swapend11(ulong value) { int len = Marshal.SizeOf(value); byte[] temp = BitConverter.GetBytes(value); Array.Reverse(temp); return BitConverter.ToUInt64(temp, 0); } public float swapend11(float value) { byte[] temp = BitConverter.GetBytes(value); if (temp[0] == 0xff) temp[0] = 0xfe; Array.Reverse(temp); return BitConverter.ToSingle(temp, 0); } public int swapend11(int value) { int len = Marshal.SizeOf(value); byte[] temp = BitConverter.GetBytes(value); Array.Reverse(temp); return BitConverter.ToInt32(temp, 0); } public double swapend11(double value) { int len = Marshal.SizeOf(value); byte[] temp = BitConverter.GetBytes(value); Array.Reverse(temp); return BitConverter.ToDouble(temp, 0); } } }