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 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 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 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); } }
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))); }
// 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); }