//PC--->>>plane 发送数据 private void generatePacket(byte messageType, object indata) { byte[] data = MavlinkUtil.StructureToByteArray(indata); //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); byte[] packet = new byte[data.Length + 6 + 2]; packet[0] = 254; packet[1] = (byte)data.Length; packet[2] = (byte)packet_count; packet_count++; packet[3] = SYS_ID; // 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 = MAVLink.MavlinkCRC.crc_calculate(packet, packet[1] + 6); checksum = MAVLink.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; packet[i + 1] = ck_b; //if (this.serial.isComOpen) if (this.serial.IsOpen) { this.serial.Write(packet, 0, packet.Length); } }
public async Task <InvokeResult> SendMessage(IDrone drone, MAVLINK_MSG_ID messageId, object req) { var buffer = MavlinkUtil.GeneratePacket(drone, MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req); await _serialPort.WriteAsync(buffer); return(InvokeResult.Success); }
void generatePacket(byte messageType, object indata) { if (!BaseStream.IsOpen) { return; } lock (writelock) { byte[] data; data = MavlinkUtil.StructureToByteArray(indata); byte[] packet = new byte[data.Length + 6 + 2]; packet[0] = 254; packet[1] = (byte)data.Length; packet[2] = (byte)packetcount; packetcount++; packet[3] = 255; // this is always 255 - MYGCS packet[4] = (byte)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_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) { BaseStream.Write(packet, 0, i); _bytesSentSubj.OnNext(i); } } }
public void ByteArrayToStructure() { var array = new byte[100]; for (var l = 0; l < array.Length; l++) { array[l] = (byte)l; } var obj = (object)new MAVLink.mavlink_heartbeat_t(); int a = 0; for (a = 0; a < 1000000; a++) { MavlinkUtil.ByteArrayToStructure(array, ref obj, 6, 5); } }
public void ReadUsingPointer() { var array = new byte[100]; for (var l = 0; l < array.Length; l++) { array[l] = (byte)l; } int a = 0; for (a = 0; a < 1000000; a++) { var ans2 = MavlinkUtil.ReadUsingPointer <MAVLink.mavlink_heartbeat_t>(array, 6); } }
public void ByteArrayToStructureGCT() { var array = new byte[100]; for (var l = 0; l < array.Length; l++) { array[l] = (byte)l; } int a = 0; for (a = 0; a < 1000000; a++) { var ans3 = MavlinkUtil.ByteArrayToStructureGC <MAVLink.mavlink_heartbeat_t>(array, 6); } }
public byte[] GenerateMAVLinkPacket_PX4(MAVLINK_MSG_ID messageType, object indata) { byte[] data; data = MavlinkUtil.StructureToByteArray(indata); byte[] packet = new byte[data.Length + 10 + 2]; packet[0] = 0xfd; packet[1] = (byte)data.Length; packet[2] = 0; packet[3] = 0; packet[4] = 0x75; packet[5] = 255; // this is always 255 - MYGCS packet[6] = 0; packet[7] = (byte)messageType; //messageType packet[8] = 0; //messageType extera packet[9] = 0; //messageType extera packetcount++; //packet[3] = 255; // this is always 255 - MYGCS //packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; //packet[5] = (byte)messageType; int i = 10; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 10); checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[(byte)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); }
public void ByteArrayToStructureGC() { var array = new byte[100]; for (var l = 0; l < array.Length; l++) { array[l] = (byte)l; } int a = 0; for (a = 0; a < 1000000; a++) { var ans4 = MavlinkUtil.ByteArrayToStructureGC(array, typeof(MAVLink.mavlink_heartbeat_t), 6, 5); } }
public MavLinkMessage(byte[] buffer) { // fill out standard fields header = buffer[0]; length = (int)buffer[1]; seq = (int)buffer[2]; sysid = (int)buffer[3]; compid = (int)buffer[4]; messid = (MAVLink.MAVLINK_MSG_ID)buffer[5]; //create the object specified by the packet type object data = Activator.CreateInstance(getMessageType()); // fill in the data of the object MavlinkUtil.ByteArrayToStructure(buffer, ref data, 6); data_struct = data; }
private void but_structtest_Click(object sender, EventArgs e) { var array = new byte[100]; for (var l = 0; l < array.Length; l++) { array[l] = (byte)l; } var a = 0; var start = DateTime.MinValue; var end = DateTime.MinValue; start = DateTime.Now; for (a = 0; a < 1000000; a++) { var obj = (object)new MAVLink.mavlink_heartbeat_t(); MavlinkUtil.ByteArrayToStructure(array, ref obj, 6); } end = DateTime.Now; Console.WriteLine("ByteArrayToStructure " + (end - start).TotalMilliseconds); start = DateTime.Now; for (a = 0; a < 1000000; a++) { var ans1 = MavlinkUtil.ByteArrayToStructureT <MAVLink.mavlink_heartbeat_t>(array, 6); } end = DateTime.Now; Console.WriteLine("ByteArrayToStructureT<> " + (end - start).TotalMilliseconds); start = DateTime.Now; for (a = 0; a < 1000000; a++) { var ans2 = MavlinkUtil.ReadUsingPointer <MAVLink.mavlink_heartbeat_t>(array, 6); } end = DateTime.Now; Console.WriteLine("ReadUsingPointer " + (end - start).TotalMilliseconds); start = DateTime.Now; for (a = 0; a < 1000000; a++) { var ans3 = MavlinkUtil.ByteArrayToStructureGC <MAVLink.mavlink_heartbeat_t>(array, 6); } end = DateTime.Now; Console.WriteLine("ByteArrayToStructureGC " + (end - start).TotalMilliseconds); }
public byte[] GenerateMAVLinkPacket10(MAVLINK_MSG_ID messageType, object indata, byte sysid = 255, byte compid = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER, int sequence = -1) { byte[] data; data = MavlinkUtil.StructureToByteArray(indata); byte[] packet = new byte[data.Length + 6 + 2]; packet[0] = MAVLINK_STX_MAVLINK1; packet[1] = (byte)data.Length; packet[2] = (byte)packetcount; if (sequence != -1) { packet[2] = (byte)sequence; } packetcount++; packet[3] = sysid; // this is always 255 - MYGCS packet[4] = compid; packet[5] = (byte)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_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, 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); }
public MAVLinkMessage ReadPacketObj(Stream BaseStream) { byte[] buffer = ReadPacket(BaseStream); byte header = buffer[0]; byte length = buffer[1]; byte seq = buffer[2]; byte sysid = buffer[3]; byte compid = buffer[4]; byte messid = buffer[5]; //create the object specified by the packet type object data = Activator.CreateInstance(MAVLINK_MESSAGE_INFO[messid]); // fill in the data of the object MavlinkUtil.ByteArrayToStructure(buffer, ref data, 6); return(new MAVLinkMessage(header, length, seq, sysid, compid, messid, data)); }
public byte[] GenerateMAVLinkPacket(MAVLINK_MSG_ID messageType, object indata) { byte[] data; data = MavlinkUtil.StructureToByteArray(indata); byte[] packet = new byte[data.Length + 6 + 2]; packet[0] = 254; packet[1] = (byte)data.Length; packet[2] = (byte)packetcount; packetcount++; packet[3] = 255; // this is always 255 - MYGCS packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_Application1; packet[5] = (byte)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_MESSAGE_CRCS[(byte)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); }
public async Task <InvokeResult <TMavlinkPacket> > WaitForMessageAsync <TMavlinkPacket>(MAVLINK_MSG_ID messageId, TimeSpan timeout) where TMavlinkPacket : struct { try { Debug.WriteLine("Start Waiting" + DateTime.Now.ToString()); var wor = new WaitOnRequest <object>((uint)messageId); Sessions[(uint)messageId] = wor; MAVLinkMessage message = null; for (var idx = 0; (idx < timeout.TotalMilliseconds / 100) && message == null; ++idx) { if (wor.CompletionSource.Task.IsCompleted) { Debug.WriteLine("TASK IS COMPLETED!!!!"); message = wor.CompletionSource.Task.Result as MAVLinkMessage; } await Task.Delay(100); } Debug.WriteLine("End Waiting" + DateTime.Now.ToString()); if (message == null) { return(InvokeResult <TMavlinkPacket> .FromError("Timeout waiting for message.")); } else { var result = MavlinkUtil.ByteArrayToStructure <TMavlinkPacket>(message.buffer); return(InvokeResult <TMavlinkPacket> .Create(result)); } } catch (Exception ex) { return(InvokeResult <TMavlinkPacket> .FromException("AsyncCoupler_WaitOnAsync", ex)); } finally { Sessions.TryRemove((uint)messageId, out WaitOnRequest <Object> obj); } }
// Parse a single Mavnet message from a slice of a byte array private MavnetMessage ParseSinglePacket(Span <byte> pkt) { MavnetMessage msg = new MavnetMessage(); // Parse out the header if (pkt[0] == MAVLink.MAVLINK_STX) { msg.is_mavlink_v2 = true; msg.payload_length = pkt[1]; msg.incompat_flags = pkt[2]; msg.compat_flags = pkt[3]; msg.sequence = pkt[4]; msg.system_id = pkt[5]; msg.component_id = pkt[6]; msg.message_id = (uint)((pkt[9] << 16) + (pkt[8] << 8) + pkt[7]); if ((msg.incompat_flags & MAVLink.MAVLINK_IFLAG_SIGNED) > 0) { Span <byte> sig_block = pkt.Slice(pkt.Length - MAVLink.MAVLINK_SIGNATURE_BLOCK_LEN); msg.signature_link_id = sig_block[0]; msg.signature_timestamp = BitConverter.ToUInt64(sig_block.Slice(1, 6)); msg.signature = sig_block.Slice(7).ToArray(); } } else { msg.is_mavlink_v2 = false; msg.payload_length = pkt[1]; msg.sequence = pkt[2]; msg.system_id = pkt[3]; msg.component_id = pkt[4]; msg.message_id = pkt[5]; } // Lookup the message type info. MAVLink.message_info messageInfo; if (!_messageInfoTable.TryGetValue(msg.message_id, out messageInfo)) { log("Unknown MAVLink message ID: " + msg.message_id); unknownMessageTypes++; return(null); } msg.message_name = messageInfo.name; // Check the CRC var headerLen = msg.is_mavlink_v2 ? MAVLink.MAVLINK_CORE_HEADER_LEN : MAVLink.MAVLINK_CORE_HEADER_MAVLINK1_LEN; var crc1 = headerLen + msg.payload_length + 1; var crc2 = crc1 + 1; ushort crc16 = (ushort)((pkt[crc2] << 8) + pkt[crc1]); ushort calcCrc = MAVLink.MavlinkCRC.crc_calculate(pkt.ToArray(), pkt.Length - 2); calcCrc = MAVLink.MavlinkCRC.crc_accumulate(messageInfo.crc, calcCrc); if (crc16 != calcCrc) { log(string.Format("Bad message CRC for {0} message: expected: {1}, found: {2} ", msg.message_name, calcCrc, crc16)); crcErrors++; return(null); } // Deserialize the payload try { object payload = Activator.CreateInstance(messageInfo.type); if (msg.is_mavlink_v2) { MavlinkUtil.ByteArrayToStructure(pkt.ToArray(), ref payload, MAVLink.MAVLINK_NUM_HEADER_BYTES, msg.payload_length); } else { MavlinkUtil.ByteArrayToStructure(pkt.ToArray(), ref payload, 6, msg.payload_length); } msg.payload = payload; } catch (Exception ex) { log(ex.ToString()); return(null); } return(msg); }
public byte[] GenerateMAVLinkPacket20(MAVLINK_MSG_ID messageType, object indata, bool sign = false, byte sysid = 255, byte compid = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER, int sequence = -1) { byte[] data; data = MavlinkUtil.StructureToByteArray(indata); MavlinkUtil.trim_payload(ref data); int extra = 0; if (sign) { extra = MAVLINK_SIGNATURE_BLOCK_LEN; } byte[] packet = new byte[data.Length + MAVLINK_NUM_NON_PAYLOAD_BYTES + extra]; packet[0] = MAVLINK_STX; packet[1] = (byte)data.Length; packet[2] = 0;//incompat signing if (sign) { packet[2] |= MAVLINK_IFLAG_SIGNED; } packet[3] = 0;//compat packet[4] = (byte)packetcount; if (sequence != -1) { packet[4] = (byte)sequence; } packetcount++; packet[5] = sysid; packet[6] = compid; packet[7] = (byte)((UInt32)messageType); packet[8] = (byte)((UInt32)messageType >> 8); packet[9] = (byte)((UInt32)messageType >> 16); int i = MAVLINK_NUM_HEADER_BYTES; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = MavlinkCRC.crc_calculate(packet, data.Length + MAVLINK_NUM_HEADER_BYTES); checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, 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 (sign) { //https://docs.google.com/document/d/1ETle6qQRcaNWAmpG2wz0oOpFKSF_bcTmYMQvtTGI8ns/edit /* * 8 bits of link ID * 48 bits of timestamp * 48 bits of signature */ // signature = sha256_48(secret_key + header + payload + CRC + link-ID + timestamp) var timestamp = (UInt64)((DateTime.UtcNow - new DateTime(2015, 1, 1)).TotalMilliseconds * 100); if (timestamp == lasttimestamp) { timestamp++; } lasttimestamp = timestamp; var timebytes = BitConverter.GetBytes(timestamp); var sig = new byte[7]; // 13 includes the outgoing hash sig[0] = sendlinkid; Array.Copy(timebytes, 0, sig, 1, 6); // timestamp //Console.WriteLine("gen linkid {0}, time {1} {2} {3} {4} {5} {6} {7}", sig[0], sig[1], sig[2], sig[3], sig[4], sig[5], sig[6], timestamp); if (signingKey == null || signingKey.Length != 32) { signingKey = new byte[32]; } using (SHA256 signit = SHA256.Create()) { MemoryStream ms = new MemoryStream(); ms.Write(signingKey, 0, signingKey.Length); ms.Write(packet, 0, i); ms.Write(sig, 0, sig.Length); var ctx = signit.ComputeHash(ms.GetBuffer()); // trim to 48 Array.Resize(ref ctx, 6); foreach (byte b in sig) { packet[i] = b; i++; } foreach (byte b in ctx) { packet[i] = b; i++; } } } return(packet); }
private bool process_message() { Console.WriteLine("packet: " + gsof_msg.packettype.ToString("X") + " len: " + gsof_msg.length + " status: " + gsof_msg.status); if (gsof_msg.packettype == 0x57) // RAWDATA { uint8_t record_type = gsof_msg.data[0]; uint8_t page_total = (byte)(gsof_msg.data[1] & 0xf); uint8_t page_number = (byte)((gsof_msg.data[1] >> 4) & 0xf); uint8_t reply_number = (byte)gsof_msg.data[2]; uint8_t recordintflags = gsof_msg.data[3]; bool consise = (recordintflags & 0x1) > 0; Console.WriteLine(DateTime.Now.Second + " record type: " + record_type + " page_no " + page_number + "/" + page_total + " consise " + consise); if (record_type == 7) { double p40 = Math.Pow(2, 40); double p39 = Math.Pow(2, 39); double p11 = Math.Pow(2, 11); double p12 = Math.Pow(2, 12); double p14 = Math.Pow(2, 14); double p21 = Math.Pow(2, 21); double p26 = Math.Pow(2, 26); double p17 = Math.Pow(2, 17); double p4 = Math.Pow(2, 4); var test2 = (rt27_7_29)gsof_msg.data.ByteArrayToStructureBigEndian <rt27_7_29>(4); long lat = byte2long(test2.Lat); long lng = byte2long(test2.Lng); Console.WriteLine("{0} {1} {2} {3} {4} {5} ", lat / p40, lng / p39, test2.Alt / p12, (test2.veln / p21), (test2.vele / p21), (test2.velu / p21), test2.rx_clock_offset / p26, test2.rx_clock_drift / p17, test2.hdop / p4); } else if (record_type == 1) { var test2 = (rt27_1_11)gsof_msg.data.ByteArrayToStructureBigEndian <rt27_1_11>(4); Console.WriteLine("{0} {1} {2}", ToDeg(test2.Latitude * Math.PI), ToDeg(test2.Longitude * Math.PI), test2.Altitude); } else if (record_type == 2) { var test2 = (rt27_2_19)gsof_msg.data.ByteArrayToStructureBigEndian <rt27_2_19>(4); Console.WriteLine("Event {0} {1} {2} {3}", test2.Source, test2.Port, test2.Number, test2.TOW); } else if (record_type == 6) // there is no documentation on this { if (page_number == 1) { obsdata.Initialize(); Array.ConstrainedCopy(gsof_msg.data, 0, obsdata, 0, gsof_msg.length); } else { Array.ConstrainedCopy(gsof_msg.data, 4, obsdata, 248 + (page_number - 2) * 244, gsof_msg.length - 4); } if (page_number == page_total) { var m_epochHeader = (rt27_6_27_epochHeader) obsdata.ByteArrayToStructureBigEndian <rt27_6_27_epochHeader>(4); var head_len = m_epochHeader.blockLength; var m_sysOffsets = new rt27_6_27_sysOffsets(); var obj = m_sysOffsets as object; MavlinkUtil.ByteArrayToStructureEndian(obsdata, ref obj, (4 + head_len)); m_sysOffsets = (rt27_6_27_sysOffsets)obj; var sysoffset_lemn = m_sysOffsets.blockLength; var m_measurement = new List <rt27_6_27_measurement>(); var SVs = m_epochHeader.nSVs; var offset = 4 + head_len + sysoffset_lemn; for (int index = 0; index < SVs; ++index) { var measheader = new rt27_6_27_measurementHeader(); obj = measheader as object; MavlinkUtil.ByteArrayToStructureEndian(obsdata, ref obj, offset); measheader = (rt27_6_27_measurementHeader)obj; var meashead_len = measheader.blockLength; offset += meashead_len; string type = ""; switch (measheader.svType) { case 0: type = "Gps"; break; case 1: type = "Sbas"; break; case 2: type = "Glonass"; break; case 3: type = "Galileo"; break; case 4: type = "QZSS"; break; case 7: type = "beidou"; break; } Console.WriteLine("prn {0} type {1} el {2} az {3} blocks {4}", measheader.prn, type, (double)(sbyte)measheader.elevation, (double)measheader.azimuth / Math.Pow(2.0, -1.0), measheader.nBlocks); for (int a = 0; a < measheader.nBlocks; a++) { var measblock = new rt27_6_27_measurementBlock(); obj = measblock as object; MavlinkUtil.ByteArrayToStructureEndian(obsdata, ref obj, offset); measblock = (rt27_6_27_measurementBlock)obj; if (a == 0) { Console.WriteLine("{0} {1}", measblock.pseudorange / Math.Pow(2.0, 7.0), byte2long(measblock.phase) / Math.Pow(2.0, 15.0)); } else { } var measblock_len = measblock.blockLength; offset += measblock_len; m_measurement.Add(new rt27_6_27_measurement() { block = measblock, header = measheader }); } } } } } if (gsof_msg.packettype == 0x40) // GSOF { uint8_t trans_number = gsof_msg.data[0]; uint8_t pageidx = gsof_msg.data[1]; uint8_t maxpageidx = gsof_msg.data[2]; //http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html //http://www.trimble.com/EC_ReceiverHelp/V4.19/en/GSOFmessages_Overview.htm Console.WriteLine("GSOF page: " + pageidx + " of " + maxpageidx); // want 2 8 9 38 for (uint a = 3; a < gsof_msg.length; a++) { uint8_t output_type = gsof_msg.data[a]; a++; uint8_t output_length = gsof_msg.data[a]; a++; Console.WriteLine("GSOF type: " + output_type + " len: " + output_length); if (output_type == 2) // position { state.location.lat = (int32_t)(ToDeg(SwapDouble(gsof_msg.data, a)) * 1e7); state.location.lng = (int32_t)(ToDeg(SwapDouble(gsof_msg.data, a + 8)) * 1e7); state.location.alt = (int32_t)(SwapDouble(gsof_msg.data, a + 16) * 1e2); state.last_gps_time_ms = state.time_week_ms; } else if (output_type == 8) // velocity { uint8_t vflag = gsof_msg.data[a]; if ((vflag & 1) == 1) { state.ground_speed = SwapFloat(gsof_msg.data, a + 1); state.ground_course = (float)(ToDeg(SwapFloat(gsof_msg.data, a + 5))); fill_3d_velocity(); state.velocity.z = -SwapFloat(gsof_msg.data, a + 9); state.have_vertical_velocity = true; } } else if (output_type == 9) //dop { state.hdop = (uint16_t)(SwapFloat(gsof_msg.data, a + 4) * 100); } else if (output_type == 12) // position sigma { state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8)) / 2; state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16); state.have_horizontal_accuracy = true; state.have_vertical_accuracy = true; } else if (output_type == 1) // pos time { state.time_week_ms = SwapUint32(gsof_msg.data, a); state.time_week = SwapUint16(gsof_msg.data, a + 4); state.num_sats = gsof_msg.data[a + 6]; uint8_t posf1 = gsof_msg.data[a + 7]; uint8_t posf2 = gsof_msg.data[a + 8]; Console.WriteLine("POSTIME: " + posf1 + " " + posf2); if ((posf1 & 1) == 1) { state.status = AP_GPS.GPS_OK_FIX_3D; if ((posf2 & 1) == 1) { state.status = AP_GPS.GPS_OK_FIX_3D_DGPS; if ((posf2 & 4) == 4) { state.status = AP_GPS.GPS_OK_FIX_3D_RTK; } } } else { state.status = AP_GPS.NO_FIX; } } a += output_length - 1u; } Type t = state.GetType(); //where obj is object whose properties you need. FieldInfo[] pi = t.GetFields(); foreach (var p in pi) { System.Console.WriteLine(p.Name + " " + p.GetValue(state).ToString()); } return(true); } return(false); }
/// <summary> /// Serialize this message to MAVLink v2 byte message. /// </summary> public byte[] toBytes(byte [] signingKey = null) { byte[] payload_bytes = MavlinkUtil.StructureToByteArray(payload); // Truncate zero bytes at the end of the payload var length = payload_bytes.Length; while (length > 1 && payload_bytes[length - 1] == 0) { length--; } Span <byte> data = new Span <byte>(payload_bytes, 0, length); int extra = 0; if (signingKey != null) { extra = MAVLink.MAVLINK_SIGNATURE_BLOCK_LEN; } byte[] packet = new byte[data.Length + MAVLink.MAVLINK_NUM_NON_PAYLOAD_BYTES + extra]; packet[0] = MAVLink.MAVLINK_STX; packet[1] = (byte)data.Length; packet[2] = 0; //incompat signing if (signingKey != null) { packet[2] |= MAVLink.MAVLINK_IFLAG_SIGNED; } packet[3] = 0; //compat packet[4] = sequence; packet[5] = system_id; packet[6] = component_id; packet[7] = (byte)message_id; packet[8] = (byte)(message_id >> 8); packet[9] = (byte)(message_id >> 16); int i = MAVLink.MAVLINK_NUM_HEADER_BYTES; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = MAVLink.MavlinkCRC.crc_calculate(packet, data.Length + MAVLink.MAVLINK_NUM_HEADER_BYTES); checksum = MAVLink.MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_INFOS.GetMessageInfo(message_id).crc, 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 (signature != null) { //https://docs.google.com/document/d/1ETle6qQRcaNWAmpG2wz0oOpFKSF_bcTmYMQvtTGI8ns/edit /* * 8 bits of link ID * 48 bits of timestamp * 48 bits of signature */ // signature = sha256_48(secret_key + header + payload + CRC + link-ID + timestamp) var timebytes = BitConverter.GetBytes(signature_timestamp); var sig = new byte[7]; // 13 includes the outgoing hash sig[0] = signature_link_id; Array.Copy(timebytes, 0, sig, 1, 6); // timestamp using (SHA256 signit = SHA256.Create()) { MemoryStream ms = new MemoryStream(); ms.Write(signingKey, 0, signingKey.Length); ms.Write(packet, 0, i); ms.Write(sig, 0, sig.Length); var ctx = signit.ComputeHash(ms.ToArray()); // trim to 48 Array.Resize(ref ctx, 6); foreach (byte b in sig) { packet[i] = b; i++; } foreach (byte b in ctx) { packet[i] = b; i++; } } } return(packet); }
public object ReadPacket(Stream BaseStream) { byte[] buffer = new byte[270]; int readcount = 0; while (readcount < 200) { // read STX byte BaseStream.Read(buffer, 0, 1); if (buffer[0] == MAVLink.MAVLINK_STX) { break; } readcount++; } // read header int read = BaseStream.Read(buffer, 1, 5); // packet length int lengthtoread = buffer[1] + 6 + 2 - 2; // data + header + checksum - STX - length //read rest of packet read = BaseStream.Read(buffer, 6, lengthtoread - 4); // resize the packet to the correct length Array.Resize <byte>(ref buffer, lengthtoread + 2); // calc crc ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2); // calc extra bit of crc for mavlink 1.0 if (buffer.Length > 5 && buffer[0] == 254) { crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[buffer[5]], crc); } // check message length vs table if (buffer.Length > 5 && buffer[1] != MAVLINK_MESSAGE_LENGTHS[buffer[5]]) { // bad or unknown packet return(null); } // check crc if (buffer.Length < 5 || buffer[buffer.Length - 1] != (crc >> 8) || buffer[buffer.Length - 2] != (crc & 0xff)) { // crc fail return(null); } byte header = buffer[0]; byte length = buffer[1]; byte seq = buffer[2]; byte sysid = buffer[3]; byte compid = buffer[4]; byte messid = buffer[5]; //create the object spcified by the packet type object data = Activator.CreateInstance(MAVLINK_MESSAGE_INFO[messid]); // fill in the data of the object MavlinkUtil.ByteArrayToStructure(buffer, ref data, 6); return(data); }
public Task UpdateDroneAsync(IDrone drone, MAVLink.MAVLinkMessage msg) { switch ((MAVLink.MAVLINK_MSG_ID)msg.msgid) { case MAVLink.MAVLINK_MSG_ID.HEARTBEAT: var hb = MavlinkUtil.ByteArrayToStructure <mavlink_heartbeat_t>(msg.payload); break; case MAVLINK_MSG_ID.RAW_IMU: var imu = MavlinkUtil.ByteArrayToStructure <mavlink_raw_imu_t>(msg.payload); drone.Acc.X = imu.xacc; drone.Acc.Y = imu.yacc; drone.Acc.Z = imu.zacc; drone.Gyro.X = imu.xgyro; drone.Gyro.Y = imu.ygyro; drone.Gyro.Z = imu.zgyro; drone.Magnometer.X = imu.xmag; drone.Magnometer.Y = imu.xmag; drone.Magnometer.Z = imu.zmag; break; case MAVLINK_MSG_ID.ATTITUDE: var att = MavlinkUtil.ByteArrayToStructure <mavlink_attitude_t>(msg.payload); drone.Pitch = att.pitch; drone.Roll = att.roll; drone.Yaw = att.yaw; drone.PitchSpeed = att.pitchspeed; drone.RollSpeed = att.rollspeed; drone.YawSpeed = att.yawspeed; break; case MAVLINK_MSG_ID.RC_CHANNELS: var rc = MavlinkUtil.ByteArrayToStructure <mavlink_rc_channels_raw_t>(msg.payload); drone.Channels[0].Value = rc.chan1_raw; drone.Channels[1].Value = rc.chan2_raw; drone.Channels[2].Value = rc.chan3_raw; drone.Channels[3].Value = rc.chan4_raw; drone.Channels[4].Value = rc.chan5_raw; drone.Channels[5].Value = rc.chan6_raw; drone.Channels[6].Value = rc.chan7_raw; drone.Channels[7].Value = rc.chan8_raw; break; case MAVLINK_MSG_ID.GLOBAL_POSITION_INT: var gpsint = MavlinkUtil.ByteArrayToStructure <mavlink_global_position_int_t>(msg.payload); var location = new GeoLocation() { Longitude = gpsint.lon / 1000000.0f, Latitude = gpsint.lat / 1000000.0f, Altitude = gpsint.alt / 1000.0f }; drone.Location = location; break; case MAVLINK_MSG_ID.SERVO_OUTPUT_RAW: var srvout = MavlinkUtil.ByteArrayToStructure <mavlink_servo_output_raw_t>(msg.payload); drone.ServoOutputs[0].Value = srvout.servo1_raw; drone.ServoOutputs[1].Value = srvout.servo2_raw; drone.ServoOutputs[2].Value = srvout.servo3_raw; drone.ServoOutputs[3].Value = srvout.servo4_raw; drone.ServoOutputs[4].Value = srvout.servo5_raw; drone.ServoOutputs[5].Value = srvout.servo6_raw; drone.ServoOutputs[6].Value = srvout.servo7_raw; drone.ServoOutputs[7].Value = srvout.servo8_raw; drone.ServoOutputs[8].Value = srvout.servo9_raw; drone.ServoOutputs[9].Value = srvout.servo10_raw; drone.ServoOutputs[10].Value = srvout.servo11_raw; drone.ServoOutputs[11].Value = srvout.servo12_raw; drone.ServoOutputs[12].Value = srvout.servo13_raw; drone.ServoOutputs[13].Value = srvout.servo14_raw; drone.ServoOutputs[14].Value = srvout.servo15_raw; drone.ServoOutputs[15].Value = srvout.servo16_raw; break; case MAVLINK_MSG_ID.MISSION_ACK: break; case MAVLINK_MSG_ID.MISSION_CLEAR_ALL: break; case MAVLINK_MSG_ID.MISSION_COUNT: break; case MAVLINK_MSG_ID.MISSION_CURRENT: break; case MAVLINK_MSG_ID.MISSION_ITEM: break; case MAVLINK_MSG_ID.MISSION_ITEM_INT: break; case MAVLINK_MSG_ID.MISSION_ITEM_REACHED: break; case MAVLINK_MSG_ID.MISSION_REQUEST: break; case MAVLINK_MSG_ID.MISSION_REQUEST_INT: break; case MAVLINK_MSG_ID.MISSION_REQUEST_LIST: break; case MAVLINK_MSG_ID.MISSION_REQUEST_PARTIAL_LIST: break; case MAVLINK_MSG_ID.MISSION_SET_CURRENT: break; case MAVLINK_MSG_ID.MISSION_WRITE_PARTIAL_LIST: break; } //msg.MessageInfo.Type return(Task.FromResult(default(Object))); }
/// <summary> /// Generate a Mavlink Packet and write to serial /// </summary> /// <param name="messageType">type number = MAVLINK_MSG_ID</param> /// <param name="indata">struct of data</param> public static byte[] GeneratePacket(IDrone drone, int messageType, object indata, bool forcemavlink2 = false, bool forcesigning = false) { lock (objLock) { var data = MavlinkUtil.StructureToByteArray(indata); var packet = new byte[0]; int i = 0; // are we mavlink2 enabled for this sysid/compid if (!drone.MavLink2 && messageType < 256 && !forcemavlink2) { var info = MAVLink.MAVLINK_MESSAGE_INFOS.SingleOrDefault(p => p.MsgId == messageType); if (data.Length != info.minlength) { Array.Resize(ref data, (int)info.minlength); } //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); packet = new byte[data.Length + 6 + 2]; packet[0] = MAVLINK1_STX; packet[1] = (byte)data.Length; packet[2] = (byte)_packetCount; _packetCount++; packet[3] = gcssysid; packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; packet[5] = (byte)messageType; 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_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, 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; } else { // trim packet for mavlink2 MavlinkUtil.TrimePayload(ref data); packet = new byte[data.Length + MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN]; packet[0] = MAVLINK2_STX; packet[1] = (byte)data.Length; packet[2] = 0; // incompat if (drone.Signing || forcesigning) // current mav { packet[2] |= MAVLINK_IFLAG_SIGNED; } packet[3] = 0; // compat packet[4] = (byte)_packetCount; _packetCount++; packet[5] = gcssysid; packet[6] = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; packet[7] = (byte)(messageType & 0xff); packet[8] = (byte)((messageType >> 8) & 0xff); packet[9] = (byte)((messageType >> 16) & 0xff); i = 10; foreach (byte b in data) { packet[i] = b; i++; } ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + MAVLINK_NUM_HEADER_BYTES); checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, 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 (drone.Signing || forcesigning) { //https://docs.google.com/document/d/1ETle6qQRcaNWAmpG2wz0oOpFKSF_bcTmYMQvtTGI8ns/edit /* * 8 bits of link ID * 48 bits of timestamp * 48 bits of signature */ // signature = sha256_48(secret_key + header + payload + CRC + link-ID + timestamp) var timestamp = (UInt64)((DateTime.UtcNow - new DateTime(2015, 1, 1)).TotalMilliseconds * 100); if (timestamp == drone.TimeStamp) { timestamp++; } drone.TimeStamp = timestamp; var timebytes = BitConverter.GetBytes(timestamp); var sig = new byte[7]; // 13 includes the outgoing hash sig[0] = drone.SendLinkId; Array.Copy(timebytes, 0, sig, 1, 6); // timestamp //Console.WriteLine("gen linkid {0}, time {1} {2} {3} {4} {5} {6} {7}", sig[0], sig[1], sig[2], sig[3], sig[4], sig[5], sig[6], timestamp); var signingKey = drone.SigningKey; if (signingKey == null || signingKey.Length != 32) { signingKey = new byte[32]; } using (SHA256Managed signit = new SHA256Managed()) { signit.TransformBlock(signingKey, 0, signingKey.Length, null, 0); signit.TransformBlock(packet, 0, i, null, 0); signit.TransformFinalBlock(sig, 0, sig.Length); var ctx = signit.Hash; // trim to 48 Array.Resize(ref ctx, 6); foreach (byte b in sig) { packet[i] = b; i++; } foreach (byte b in ctx) { packet[i] = b; i++; } } } } return(packet); } }