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