public void OnMavMessageData(MAVLinkMessage message)
{
if (MavMessageHandler != null)
{
MavMessageHandler(this, new MavMessageEventArgs(message));
}
}
private bool messageReceived(MAVLinkMessage arg)
{
//accept any compid, but filter sysid
if (arg.sysid != _parent.sysid)
{
return(true);
}
if (arg.msgid == (uint)MAVLINK_MSG_ID.DISTANCE_SENSOR)
{
var dist = arg.ToStructure <mavlink_distance_sensor_t>();
if (dist.current_distance >= dist.max_distance)
{
return(true);
}
if (dist.current_distance <= dist.min_distance)
{
return(true);
}
_dS.Add((MAV_SENSOR_ORIENTATION)dist.orientation, dist.current_distance, DateTime.Now, 1);
}
return(true);
}
public void addPacket(MAVLinkMessage msg)
{
lock (packetslock)
{
packets[msg.msgid] = msg;
}
}
protected void Complete(MAVLinkMessage msg)
{
if (Sessions.TryGetValue(msg.msgid, out var tcs))
{
Debug.WriteLine("hit " + msg.msgid);
tcs.CompletionSource.SetResult(msg);
}
else
{
Debug.WriteLine("miss " + msg.msgid + " " + Sessions.Count);
if (Sessions.Count > 0)
{
Debug.WriteLine("miss and first key > " + Sessions.First().Key + " " + msg.msgid + " " + (Sessions.First().Key == msg.msgid).ToString());
}
}
}
public void addPacket(MAVLinkMessage msg)
{
lock (packetslock)
{
// create queue if it does not exist
if (!packets.ContainsKey(msg.msgid))
{
packets[msg.msgid] = new Queue <MAVLinkMessage>();
}
// cleanup queue if not polling this message
while (packets[msg.msgid].Count > 5)
{
packets[msg.msgid].Dequeue();
}
packets[msg.msgid].Enqueue(msg);
}
}
private bool messageReceived(MAVLinkMessage arg)
{
//accept any compid, but filter sysid
if (arg.sysid != _parent.sysid)
{
return(true);
}
if (arg.msgid == (uint)MAVLINK_MSG_ID.DISTANCE_SENSOR)
{
var dist = arg.ToStructure <mavlink_distance_sensor_t>();
if (dist.current_distance >= dist.max_distance)
{
return(true);
}
if (dist.current_distance <= dist.min_distance)
{
return(true);
}
_dS.Add((MAV_SENSOR_ORIENTATION)dist.orientation, dist.current_distance, DateTime.Now, 3);
//Console.WriteLine("DIST " + (MAV_SENSOR_ORIENTATION)dist.orientation + " " + dist.current_distance);
if (temp.IsHandleCreated)
{
//temp.Text = arg.sysid.ToString();
temp.Invalidate();
//temp.Refresh();
}
else
{
if (!temp.IsDisposed)
{
MainV2.instance.Invoke((MethodInvoker) delegate { temp.Show(); });
}
}
}
return(true);
}
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 DebugPacket(MAVLinkMessage datin, bool PrintToConsole)
{
string text = "";
return DebugPacket(datin, ref text, PrintToConsole);
}
public MAVLinkMessage ReadPacket(Stream BaseStream)
{
byte[] buffer = new byte[MAVLink.MAVLINK_MAX_PACKET_LEN];
DateTime packettime = DateTime.MinValue;
if (hasTimestamp)
{
byte[] datearray = new byte[8];
int tem = 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);
if ((dateint / 1000 / 1000 / 60 / 60) < 9999999)
{
date1 = date1.AddMilliseconds(dateint / 1000);
packettime = date1.ToLocalTime();
}
}
int readcount = 0;
while (readcount <= MAVLink.MAVLINK_MAX_PACKET_LEN)
{
// read STX byte
ReadWithTimeout(BaseStream, buffer, 0, 1);
if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1)
{
break;
}
readcount++;
}
if (readcount >= MAVLink.MAVLINK_MAX_PACKET_LEN)
{
return(null);
throw new InvalidDataException("No header found in data");
}
var headerlength = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
var headerlengthstx = headerlength + 1;
// read header
try {
ReadWithTimeout(BaseStream, buffer, 1, headerlength);
}
catch (EndOfStreamException)
{
return(null);
}
// packet length
int lengthtoread = 0;
if (buffer[0] == MAVLINK_STX)
{
lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length
if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
{
lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN;
}
}
else
{
lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length
}
try
{
//read rest of packet
ReadWithTimeout(BaseStream, buffer, headerlengthstx, lengthtoread - (headerlengthstx - 2));
}
catch (EndOfStreamException)
{
return(null);
}
// resize the packet to the correct length
Array.Resize <byte>(ref buffer, lengthtoread + 2);
MAVLinkMessage message = new MAVLinkMessage(buffer, packettime);
// calc crc
ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);
// calc extra bit of crc for mavlink 1.0+
if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1)
{
crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc);
}
// check crc
if ((message.crc16 >> 8) != (crc >> 8) ||
(message.crc16 & 0xff) != (crc & 0xff))
{
badCRC++;
// crc fail
return(null);
}
return(message);
}
public MAVLinkMessage ReadPacket(byte[] buffer)
{
int readcount = 0;
while (readcount <= MAVLink.MAVLINK_MAX_PACKET_LEN)
{
// read STX byte
if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1)
{
break;
}
readcount++;
}
if (readcount >= MAVLink.MAVLINK_MAX_PACKET_LEN)
{
throw new InvalidDataException("No header found in data");
}
var headerlength = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
var headerlengthstx = headerlength + 1;
// read header
// packet length
int lengthtoread = 0;
if (buffer[0] == MAVLINK_STX)
{
lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length
if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
{
lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN;
}
}
else
{
lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length
}
//read rest of packet
// resize the packet to the correct length
Array.Resize <byte>(ref buffer, lengthtoread + 2);
MAVLinkMessage message = new MAVLinkMessage(buffer);
// calc crc
ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);
// calc extra bit of crc for mavlink 1.0+
if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1)
{
crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc);
}
// check crc
if ((message.crc16 >> 8) != (crc >> 8) ||
(message.crc16 & 0xff) != (crc & 0xff))
{
badCRC++;
// crc fail
return(null);
}
return(message);
}
public MAVLinkMessage ReadPacket(Stream BaseStream)
{
byte[] buffer = new byte[270];
int readcount = 0;
while (readcount < 200)
{
// read STX byte
ReadWithTimeout(BaseStream, buffer, 0, 1);
if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1)
{
break;
}
readcount++;
}
var headerlength = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
var headerlengthstx = headerlength + 1;
// read header
ReadWithTimeout(BaseStream, buffer, 1, headerlength);
// packet length
int lengthtoread = 0;
if (buffer[0] == MAVLINK_STX)
{
lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length
if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
{
lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN;
}
}
else
{
lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length
}
//read rest of packet
ReadWithTimeout(BaseStream, buffer, 6, lengthtoread - (headerlengthstx - 2));
// resize the packet to the correct length
Array.Resize <byte>(ref buffer, lengthtoread + 2);
MAVLinkMessage message = new MAVLinkMessage(buffer);
// calc crc
ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);
// calc extra bit of crc for mavlink 1.0+
if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1)
{
crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc);
}
// check crc
if ((message.crc16 >> 8) != (crc >> 8) ||
(message.crc16 & 0xff) != (crc & 0xff))
{
badCRC++;
// crc fail
return(null);
}
return(message);
}
MAVLinkMessage readlogPacketMavlink()
{
byte[] datearray = new byte[8];
bool missingtimestamp = false;
if (logplaybackfile.BaseStream is FileStream)
{
if (((FileStream) _logplaybackfile.BaseStream).Name.ToLower().EndsWith(".rlog"))
missingtimestamp = true;
}
if (!missingtimestamp)
{
int tem = 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);
try
{
// array is reversed above
if (datearray[7] == 254 || datearray[7] == 253)
{
//rewind 8bytes
logplaybackfile.BaseStream.Seek(-8, SeekOrigin.Current);
}
else
{
if ((dateint/1000/1000/60/60) < 9999999)
{
date1 = date1.AddMilliseconds(dateint/1000);
lastlogread = date1.ToLocalTime();
}
}
}
catch
{
}
}
byte[] temp = new byte[0];
byte byte0 = 0;
byte byte1 = 0;
byte byte2 = 0;
var filelength = logplaybackfile.BaseStream.Length;
var filepos = logplaybackfile.BaseStream.Position;
if(filelength == filepos)
return MAVLinkMessage.Invalid;
int length = 5;
int a = 0;
while (a < length)
{
if (filelength == filepos)
return MAVLinkMessage.Invalid;
var tempb = (byte) logplaybackfile.ReadByte();
filepos++;
switch (a)
{
case 0:
byte0 = tempb;
if (byte0 != 'U' && byte0 != 254 && byte0 != 253)
{
log.DebugFormat("logread - lost sync byte {0} pos {1}", byte0,
logplaybackfile.BaseStream.Position);
// seek to next valid
do
{
byte0 = logplaybackfile.ReadByte();
}
while (byte0 != 'U' && byte0 != 254 && byte0 != 253);
a = 1;
continue;
}
break;
case 1:
byte1 = tempb;
// handle length
{
int headerlength = byte0 == MAVLINK_STX ? 9 : 5;
int headerlengthstx = headerlength + 1;
length = byte1 + headerlengthstx + 2; // header + 2 checksum
}
break;
case 2:
byte2 = tempb;
// handle signing and mavlink2
if (byte0 == MAVLINK_STX)
{
if ((byte2 & MAVLINK_IFLAG_SIGNED) > 0)
length += MAVLINK_SIGNATURE_BLOCK_LEN;
}
// handle rest
{
temp = new byte[length];
temp[0] = byte0;
temp[1] = byte1;
temp[2] = byte2;
var readto = a + 1;
var readlength = length - (a + 1);
logplaybackfile.Read(temp, readto, readlength);
a = length;
}
break;
}
a++;
}
MAVLinkMessage tmp = new MAVLinkMessage(temp);
MAVlist[tmp.sysid, tmp.compid].cs.datetime = lastlogread;
return tmp;
}
/// <summary>
/// Used to extract mission from log file - both sent or received
/// </summary>
/// <param name="buffer">packet</param>
void getWPsfromstream(ref MAVLinkMessage buffer, byte sysid, byte compid)
{
if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_COUNT)
{
// clear old
mavlink_mission_count_t wp = buffer.ToStructure<mavlink_mission_count_t>();
if (wp.target_system == gcssysid)
{
wp.target_system = sysid;
wp.target_component = compid;
}
MAVlist[wp.target_system, wp.target_component].wps.Clear();
}
if (buffer.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM)
{
mavlink_mission_item_t wp = buffer.ToStructure<mavlink_mission_item_t>();
if (wp.target_system == gcssysid)
{
wp.target_system = sysid;
wp.target_component = compid;
}
if (wp.current == 2)
{
// guide mode wp
MAVlist[wp.target_system, wp.target_component].GuidedMode = wp;
}
else
{
MAVlist[wp.target_system, wp.target_component].wps[wp.seq] = wp;
}
Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2,
wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command);
}
if (buffer.msgid == (byte) MAVLINK_MSG_ID.RALLY_POINT)
{
mavlink_rally_point_t rallypt = buffer.ToStructure<mavlink_rally_point_t>();
if (rallypt.target_system == gcssysid)
{
rallypt.target_system = sysid;
rallypt.target_component = compid;
}
MAVlist[rallypt.target_system, rallypt.target_component].rallypoints[rallypt.idx] = rallypt;
Console.WriteLine("RP # {0} {1} {2} {3} {4}", rallypt.idx, rallypt.lat, rallypt.lng, rallypt.alt,
rallypt.break_alt);
}
if (buffer.msgid == (byte)MAVLINK_MSG_ID.CAMERA_FEEDBACK)
{
mavlink_camera_feedback_t camerapt = buffer.ToStructure<mavlink_camera_feedback_t>();
if (MAVlist[sysid, compid].camerapoints.Count == 0 || MAVlist[sysid, compid].camerapoints.Last().time_usec != camerapt.time_usec)
{
MAVlist[sysid, compid].camerapoints.Add(camerapt);
}
}
if (buffer.msgid == (byte) MAVLINK_MSG_ID.FENCE_POINT)
{
mavlink_fence_point_t fencept = buffer.ToStructure<mavlink_fence_point_t>();
if (fencept.target_system == gcssysid)
{
fencept.target_system = sysid;
fencept.target_component = compid;
}
MAVlist[fencept.target_system, fencept.target_component].fencepoints[fencept.idx] = fencept;
}
if (buffer.msgid == (byte) MAVLINK_MSG_ID.PARAM_VALUE)
{
mavlink_param_value_t value = buffer.ToStructure<mavlink_param_value_t>();
string st = System.Text.ASCIIEncoding.ASCII.GetString(value.param_id);
int pos = st.IndexOf('\0');
if (pos != -1)
{
st = st.Substring(0, pos);
}
if (MAV.apname == MAV_AUTOPILOT.ARDUPILOTMEGA)
{
MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, value.param_value, MAV_PARAM_TYPE.REAL32, (MAV_PARAM_TYPE)value.param_type);
}
else
{
MAVlist[sysid, compid].param[st] = new MAVLinkParam(st, value.param_value,
(MAV_PARAM_TYPE) value.param_type);
}
}
}
/// <summary>
/// Serial Reader to read mavlink packets. POLL method
/// </summary>
/// <returns></returns>
public MAVLinkMessage readPacket()
{
byte[] buffer = new byte[MAVLINK_MAX_PACKET_LEN + 25];
int count = 0;
int length = 0;
int readcount = 0;
BaseStream.ReadTimeout = 1200; // 1200 ms between chars - the gps detection requires this.
DateTime start = DateTime.Now;
//Console.WriteLine(DateTime.Now.Millisecond + " SR0 " + BaseStream.BytesToRead);
lock (readlock)
{
lastbad = new byte[2];
//Console.WriteLine(DateTime.Now.Millisecond + " SR1 " + BaseStream.BytesToRead);
while (BaseStream.IsOpen || logreadmode)
{
try
{
if (readcount > 300)
{
log.Info("MAVLink readpacket No valid mavlink packets");
break;
}
readcount++;
if (logreadmode)
{
buffer = readlogPacketMavlink().buffer;
if (buffer.Length == 0)
return new MAVLinkMessage();
}
else
{
// time updated for internal reference
MAV.cs.datetime = DateTime.Now;
DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);
// Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream.BytesToRead);
while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0)
{
if (DateTime.Now > to)
{
log.InfoFormat("MAVLINK: 1 wait time out btr {0} len {1}", BaseStream.BytesToRead,
length);
throw new TimeoutException("Timeout");
}
System.Threading.Thread.Sleep(1);
//Console.WriteLine(DateTime.Now.Millisecond + " SR0b " + BaseStream.BytesToRead);
}
//Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream.BytesToRead);
if (BaseStream.IsOpen)
{
BaseStream.Read(buffer, count, 1);
if (rawlogfile != null && rawlogfile.CanWrite)
rawlogfile.WriteByte(buffer[count]);
}
//Console.WriteLine(DateTime.Now.Millisecond + " SR1b " + BaseStream.BytesToRead);
}
}
catch (Exception e)
{
log.Info("MAVLink readpacket read error: " + e.ToString());
break;
}
// check if looks like a mavlink packet and check for exclusions and write to console
if (buffer[0] != 0xfe && buffer[0] != 'U' && buffer[0] != 0xfd)
{
if (buffer[0] >= 0x20 && buffer[0] <= 127 || buffer[0] == '\n' || buffer[0] == '\r')
{
// check for line termination
if (buffer[0] == '\r' || buffer[0] == '\n')
{
// check new line is valid
if (buildplaintxtline.Length > 3)
plaintxtline = buildplaintxtline;
// reset for next line
buildplaintxtline = "";
}
TCPConsole.Write(buffer[0]);
Console.Write((char)buffer[0]);
buildplaintxtline += (char)buffer[0];
}
_bytesReceivedSubj.OnNext(1);
count = 0;
lastbad[0] = lastbad[1];
lastbad[1] = buffer[0];
buffer[1] = 0;
continue;
}
// reset count on valid packet
readcount = 0;
//Console.WriteLine(DateTime.Now.Millisecond + " SR2 " + BaseStream.BytesToRead);
mavlinkv2 = buffer[0] == MAVLINK_STX ? true : false;
int headerlength = mavlinkv2 ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
int headerlengthstx = headerlength + 1;
// check for a header
if (buffer[0] == 0xfe || buffer[0] == 0xfd || buffer[0] == 'U')
{
// if we have the header, and no other chars, get the length and packet identifiers
if (count == 0 && !logreadmode)
{
DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);
while (BaseStream.IsOpen && BaseStream.BytesToRead < headerlength)
{
if (DateTime.Now > to)
{
log.InfoFormat("MAVLINK: 2 wait time out btr {0} len {1}", BaseStream.BytesToRead,
length);
throw new Exception("Timeout");
}
System.Threading.Thread.Sleep(1);
}
int read = BaseStream.Read(buffer, 1, headerlength);
count = read;
if (rawlogfile != null && rawlogfile.CanWrite)
rawlogfile.Write(buffer, 1, read);
}
// packet length
if (buffer[0] == 0xfd)
{
length = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length
if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
{
length += MAVLINK_SIGNATURE_BLOCK_LEN;
}
}
else
{
length = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length
}
if (count >= headerlength || logreadmode)
{
try
{
if (logreadmode)
{
}
else
{
DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);
while (BaseStream.IsOpen && BaseStream.BytesToRead < (length - count))
{
if (DateTime.Now > to)
{
log.InfoFormat("MAVLINK: 3 wait time out btr {0} len {1}",
BaseStream.BytesToRead, length);
break;
}
System.Threading.Thread.Sleep(1);
}
if (BaseStream.IsOpen)
{
int read = BaseStream.Read(buffer, headerlengthstx, length - (headerlengthstx-2));
if (rawlogfile != null && rawlogfile.CanWrite)
{
// write only what we read, temp is the whole packet, so 6-end
rawlogfile.Write(buffer, headerlengthstx, read);
}
}
}
count = length + 2;
}
catch
{
break;
}
break;
}
}
count++;
if (count == 299)
break;
}
//Console.WriteLine(DateTime.Now.Millisecond + " SR3 " + BaseStream.BytesToRead);
} // end readlock
// resize the packet to the correct length
Array.Resize<byte>(ref buffer, count);
// add byte count
_bytesReceivedSubj.OnNext(buffer.Length);
// update bps statistics
if (bpstime.Second != DateTime.Now.Second)
{
long btr = 0;
if (BaseStream != null && BaseStream.IsOpen)
{
btr = BaseStream.BytesToRead;
}
else if (logreadmode)
{
btr = logplaybackfile.BaseStream.Length - logplaybackfile.BaseStream.Position;
}
Console.Write("bps {0} loss {1} left {2} mem {3} \n", bps1, MAV.synclost, btr,
System.GC.GetTotalMemory(false)/1024/1024.0);
bps2 = bps1; // prev sec
bps1 = 0; // current sec
bpstime = DateTime.Now;
}
bps1 += buffer.Length;
if (buffer.Length == 0)
return new MAVLinkMessage();
MAVLinkMessage message = new MAVLinkMessage(buffer);
uint msgid = message.msgid;
message_info msginfo;
msginfo = MAVLINK_MESSAGE_INFOS.SingleOrDefault(p => p.msgid == msgid);
// calc crc
var sigsize = (message.sig != null) ? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
ushort crc = MavlinkCRC.crc_calculate(buffer, message.Length - sigsize-MAVLINK_NUM_CHECKSUM_BYTES);
// calc extra bit of crc for mavlink 1.0/2.0
if (message.header == 0xfe || message.header == 0xfd)
{
crc = MavlinkCRC.crc_accumulate(msginfo.crc, crc);
}
// check message length vs table
if (message.payloadlength != msginfo.length)
{
if (msginfo.length == 0) // pass for unknown packets
{
log.InfoFormat("unknown packet type {0}", message.msgid);
}
else
{
log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", buffer.Length, message.msgid);
if (buffer.Length == 11 && message.header == 'U' && message.msgid == 0) // check for 0.9 hb packet
{
string messagehb =
"Mavlink 0.9 Heartbeat, Please upgrade your AP, This planner is for Mavlink 1.0\n\n";
Console.WriteLine(messagehb);
if (logreadmode)
logplaybackfile.BaseStream.Seek(0, SeekOrigin.End);
throw new Exception(messagehb);
}
return new MAVLinkMessage();
}
}
// check crc
if ((message.crc16 >> 8) != (crc >> 8) ||
(message.crc16 & 0xff) != (crc & 0xff))
{
if (message.msgid != -1 && buffer.Length > 5 && msginfo.name != null)
log.InfoFormat("Mavlink Bad Packet (crc fail) len {0} crc {1} vs {4} pkno {2} {3}", buffer.Length,
crc, message.msgid, msginfo.name.ToString(),
message.crc16);
if (logreadmode)
log.InfoFormat("bad packet pos {0} ", logplaybackfile.BaseStream.Position);
return new MAVLinkMessage();
}
byte sysid = message.sysid;
byte compid = message.compid;
byte packetSeqNo = message.seq;
//check if sig was included in packet, and we are not ignoring the signature (signing isnt checked else we wont enable signing)
if (message.sig != null && !MAVlist[sysid,compid].signingignore)
{
using (SHA256Managed signit = new SHA256Managed())
{
signit.TransformBlock(signingKey, 0, signingKey.Length, null, 0);
signit.TransformFinalBlock(message.buffer, 0, message.Length - MAVLINK_SIGNATURE_BLOCK_LEN + 7);
var ctx = signit.Hash;
// trim to 48
Array.Resize(ref ctx, 6);
//Console.WriteLine("linkid {0}, time {1} {2} {3} {4} {5} {6} - {7}", message.sig[0], message.sig[1], message.sig[2], message.sig[3], message.sig[4], message.sig[5], message.sig[6], message.sigTimestamp);
for (int i = 0; i < ctx.Length; i++)
{
if (ctx[i] != message.sig[7 + i])
{
log.InfoFormat("Packet failed signature but passed crc");
return new MAVLinkMessage();
}
}
MAVlist[sysid, compid].linkid = message.sig[0];
enableSigning();
}
}
// packet is now verified
// update packet loss statistics
if (!logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < DateTime.Now)
{
MAVlist[sysid, compid].packetlosttimer = DateTime.Now;
MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost*0.8f);
MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost*0.8f);
}
else if (logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < lastlogread)
{
MAVlist[sysid, compid].packetlosttimer = lastlogread;
MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost*0.8f);
MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost*0.8f);
}
// extract wp's from stream
if (buffer.Length >= 5)
{
getWPsfromstream(ref message, sysid, compid);
}
// if its a gcs packet - dont process further
if (buffer.Length >= 5 && (sysid == 255 || sysid == 253) && logreadmode) // gcs packet
{
return message;
}
// create a state for any sysid/compid
if (!MAVlist.Contains(sysid, compid))
{
// create an item - hidden
MAVlist.AddHiddenList(sysid, compid);
}
try
{
if ((message.header == 'U' || message.header == 0xfe || message.header == 0xfd) && buffer.Length >= message.payloadlength)
{
// check if we lost pacakets based on seqno
int expectedPacketSeqNo = ((MAVlist[sysid, compid].recvpacketcount + 1)%0x100);
{
// the seconds part is to work around a 3dr radio bug sending dup seqno's
if (packetSeqNo != expectedPacketSeqNo && packetSeqNo != MAVlist[sysid, compid].recvpacketcount)
{
MAVlist[sysid, compid].synclost++; // actualy sync loss's
int numLost = 0;
if (packetSeqNo < ((MAVlist[sysid, compid].recvpacketcount + 1)))
// recvpacketcount = 255 then 10 < 256 = true if was % 0x100 this would fail
{
numLost = 0x100 - expectedPacketSeqNo + packetSeqNo;
}
else
{
numLost = packetSeqNo - expectedPacketSeqNo;
}
MAVlist[sysid, compid].packetslost += numLost;
WhenPacketLost.OnNext(numLost);
log.InfoFormat("mav {2}-{4} seqno {0} exp {3} pkts lost {1}", packetSeqNo, numLost, sysid,
expectedPacketSeqNo,compid);
}
MAVlist[sysid, compid].packetsnotlost++;
//Console.WriteLine("{0} {1}", sysid, packetSeqNo);
MAVlist[sysid, compid].recvpacketcount = packetSeqNo;
}
WhenPacketReceived.OnNext(1);
// packet stats per mav
if (double.IsInfinity(MAVlist[sysid, compid].packetspersecond[msgid]))
MAVlist[sysid, compid].packetspersecond[msgid] = 0;
MAVlist[sysid, compid].packetspersecond[msgid] = (((1000 /
((DateTime.Now -
MAVlist[sysid, compid]
.packetspersecondbuild[msgid])
.TotalMilliseconds) +
MAVlist[sysid, compid].packetspersecond[
msgid]) / 2));
MAVlist[sysid, compid].packetspersecondbuild[msgid] = DateTime.Now;
//Console.WriteLine("Packet {0}",temp[5]);
// store packet history
lock (objlock)
{
MAVlist[sysid, compid].packets[msgid] = message;
// 3dr radio status packet are injected into the current mav
if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.RADIO_STATUS ||
msgid == (byte)MAVLink.MAVLINK_MSG_ID.RADIO)
{
MAVlist[sysidcurrent, compidcurrent].packets[msgid] = message;
}
// adsb packets are forwarded and can be from any sysid/compid
if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.ADSB_VEHICLE)
{
var adsb = message.ToStructure<MAVLink.mavlink_adsb_vehicle_t>();
MainV2.instance.adsbPlanes[adsb.ICAO_address.ToString("X5")] = new MissionPlanner.Utilities.adsb.PointLatLngAltHdg(adsb.lat / 1e7, adsb.lon / 1e7, adsb.altitude / 1000, adsb.heading * 0.01f, adsb.ICAO_address.ToString("X5"), DateTime.Now);
}
}
// set seens sysid's based on hb packet - this will hide 3dr radio packets
if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
mavlink_heartbeat_t hb = message.ToStructure<mavlink_heartbeat_t>();
// not a gcs
if (hb.type != (byte) MAV_TYPE.GCS)
{
// add a seen sysid
if (!MAVlist.Contains(sysid, compid, false))
{
// ensure its set from connect or log playback
MAVlist.Create(sysid, compid);
MAVlist[sysid, compid].aptype = (MAV_TYPE) hb.type;
MAVlist[sysid, compid].apname = (MAV_AUTOPILOT) hb.autopilot;
setAPType(sysid, compid);
}
// attach to the only remote device. / default to first device seen
if (MAVlist.Count == 1)
{
sysidcurrent = sysid;
compidcurrent = compid;
}
}
}
// only process for active mav
if (sysidcurrent == sysid && compidcurrent == compid)
PacketReceived(message);
if (debugmavlink)
DebugPacket(message);
if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) // status text
{
var msg = message.ToStructure<MAVLink.mavlink_statustext_t>();
byte sev = msg.severity;
string logdata = Encoding.ASCII.GetString(msg.text);
int ind = logdata.IndexOf('\0');
if (ind != -1)
logdata = logdata.Substring(0, ind);
log.Info(DateTime.Now + " " + sev + " " + logdata);
MAVlist[sysid, compid].cs.messages.Add(logdata);
bool printit = false;
// the change of severity and the autopilot version where introduced at the same time, so any version non 0 can be used
// copter 3.4+
// plane 3.4+
if (MAVlist[sysid, compid].cs.version.Major > 0 || MAVlist[sysid, compid].cs.version.Minor >= 4)
{
if (sev <= (byte) MAV_SEVERITY.WARNING)
{
printit = true;
}
}
else
{
if (sev >= 3)
{
printit = true;
}
}
if (logdata.StartsWith("Tuning:"))
printit = true;
if (printit)
{
MAVlist[sysid, compid].cs.messageHigh = logdata;
MAVlist[sysid, compid].cs.messageHighTime = DateTime.Now;
if (MainV2.speechEngine != null &&
MainV2.speechEngine.IsReady &&
Settings.Instance["speechenable"] != null &&
Settings.Instance["speechenable"].ToString() == "True")
{
MainV2.speechEngine.SpeakAsync(logdata);
}
}
}
if (lastparamset != DateTime.MinValue && lastparamset.AddSeconds(10) < DateTime.Now)
{
lastparamset = DateTime.MinValue;
if (BaseStream.IsOpen)
{
doCommand(MAV_CMD.PREFLIGHT_STORAGE, 0, 0, 0, 0, 0, 0, 0);
}
}
getWPsfromstream(ref message, sysid, compid);
try
{
if (logfile != null && logfile.CanWrite && !logreadmode)
{
lock (logfile)
{
byte[] datearray =
BitConverter.GetBytes(
(UInt64) ((DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds*1000));
Array.Reverse(datearray);
logfile.Write(datearray, 0, datearray.Length);
logfile.Write(buffer, 0, buffer.Length);
if (msgid == 0)
{
// flush on heartbeat - 1 seconds
logfile.Flush();
rawlogfile.Flush();
}
}
}
}
catch (Exception ex)
{
log.Error(ex);
}
try
{
// full rw from mirror stream
if (MirrorStream != null && MirrorStream.IsOpen)
{
MirrorStream.Write(buffer, 0, buffer.Length);
while (MirrorStream.BytesToRead > 0)
{
byte[] buf = new byte[1024];
int len = MirrorStream.Read(buf, 0, buf.Length);
if (MirrorStreamWrite)
BaseStream.Write(buf, 0, len);
}
}
}
catch
{
}
}
}
catch (Exception ex)
{
log.Error(ex);
}
// update last valid packet receive time
MAVlist[sysid, compid].lastvalidpacket = DateTime.Now;
return new MAVLinkMessage(buffer);
}
MAVLinkMessage readlogPacketMavlink()
{
byte[] temp = new byte[300];
//byte[] datearray = BitConverter.GetBytes((ulong)(DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds);
byte[] datearray = new byte[8];
int tem = 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);
try
{
// array is reversed above
if (datearray[7] == 254 || datearray[7] == 253)
{
//rewind 8bytes
logplaybackfile.BaseStream.Seek(-8, SeekOrigin.Current);
}
else
{
if ((dateint/1000/1000/60/60) < 9999999)
{
date1 = date1.AddMilliseconds(dateint/1000);
lastlogread = date1.ToLocalTime();
}
}
}
catch
{
}
int length = 5;
int a = 0;
while (a < length)
{
if (logplaybackfile.BaseStream.Position == logplaybackfile.BaseStream.Length)
break;
temp[a] = (byte) logplaybackfile.ReadByte();
if (temp[0] != 'U' && temp[0] != 254 && temp[0] != 253)
{
log.InfoFormat("logread - lost sync byte {0} pos {1}", temp[0], logplaybackfile.BaseStream.Position);
a = 0;
continue;
}
if (a == 1)
{
int headerlength = temp[0] == MAVLINK_STX ? 9 : 5;
int headerlengthstx = headerlength + 1;
length = temp[1] + headerlengthstx + 2; // header + 2 checksum
}
if (a == 2 && temp[0] == MAVLINK_STX)
{
if ((temp[a] & MAVLINK_IFLAG_SIGNED) > 0)
length += MAVLINK_SIGNATURE_BLOCK_LEN;
}
a++;
}
MAVLinkMessage tmp = new MAVLinkMessage(temp);
MAVlist[tmp.sysid, tmp.compid].cs.datetime = lastlogread;
return tmp;
}
public MavMessageEventArgs(MAVLinkMessage message)
{
Message = message;
}
public string CollectPacket(byte[] data)
{
int dataIndex = 0;
StringBuilder nonMavData = new StringBuilder();
while (dataIndex < data.Length)
{
byte dataByte = data[dataIndex++];
if (collectCount == 0)
{
// read STX byte
collectBuffer[0] = dataByte;
if (collectBuffer[0] == MAVLink.MAVLINK_STX || collectBuffer[0] == MAVLINK_STX_MAVLINK1)
{
collectCount = 1;
collectGotHeader = false;
}
else
{
nonMavData.Append(Encoding.ASCII.GetString(new byte[] { dataByte }));
}
continue;
}
var headerlength = collectBuffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
var headerlengthstx = headerlength + 1;
if (!collectGotHeader && collectCount < headerlengthstx)
{
// read header
collectBuffer[collectCount++] = dataByte;
if (collectCount >= headerlengthstx)
{
collectGotHeader = true;
collectCount = 6;
}
continue;
}
// packet length
int lengthToRead;
if (collectBuffer[0] == MAVLINK_STX)
{
lengthToRead = collectBuffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length
if ((collectBuffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
{
lengthToRead += MAVLINK_SIGNATURE_BLOCK_LEN;
}
}
else
{
lengthToRead = collectBuffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length
}
//read rest of packet
//ReadWithTimeout(BaseStream, buffer, 6, lengthtoread - (headerlengthstx - 2));
if (collectCount < lengthToRead - (headerlengthstx - 2) + 6)
{
collectBuffer[collectCount++] = dataByte;
if (collectCount < lengthToRead - (headerlengthstx - 2) + 6)
{
continue;
}
}
MAVLinkMessage message = new MAVLinkMessage(collectBuffer);
// resize the packet to the correct length
//Array.Resize<byte>(ref collectBuffer, lengthToRead + 2);
// calc crc
ushort crc = MavlinkCRC.crc_calculate(collectBuffer, collectCount - 2);
// calc extra bit of crc for mavlink 1.0+
if ((message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1) &&
message.msgid < MAVLINK_MESSAGE_CRCS.Count)
{
crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_CRCS[message.msgid], crc);
}
// check crc
if ((message.crc16 >> 8) != (crc >> 8) ||
(message.crc16 & 0xff) != (crc & 0xff))
{
badCRC++;
OnMavError(MavError.BAD_CRC);
}
else
{
OnMavMessageData(message);
}
collectCount = 0;
}
return(nonMavData.ToString());
}
private bool messageReceived(MAVLinkMessage arg)
{
//accept any compid, but filter sysid
if (arg.sysid != _parent.sysid)
return true;
if (arg.msgid == (uint)MAVLINK_MSG_ID.DISTANCE_SENSOR)
{
var dist = arg.ToStructure<mavlink_distance_sensor_t>();
if (dist.current_distance >= dist.max_distance)
return true;
if (dist.current_distance <= dist.min_distance)
return true;
_dS.Add(dist.id, (MAV_SENSOR_ORIENTATION)dist.orientation, dist.current_distance, DateTime.Now, 3);
if (temp.IsHandleCreated)
{
temp.Invalidate();
}
else
{
if (!temp.IsDisposed)
{
MainV2.instance.Invoke((MethodInvoker)delegate { temp.Show(); });
}
}
}
return true;
}
private void LoadFile(object sender, EventArgs e)
{
using (OpenFileDialog ofdFile = new OpenFileDialog())
{
ofdFile.Filter = GAP.filemask;
ofdFile.FilterIndex = 2;
ofdFile.RestoreDirectory = true;
if (ofdFile.ShowDialog() == System.Windows.Forms.DialogResult.OK)
{
GAP.AllTlogData.Clear();
foreach (string logfile in ofdFile.FileNames)
{
string extension = Path.GetExtension(logfile);
if (String.Equals(extension.ToLower(), ".tlog"))
{
progressbar1.Visibility = Visibility.Visible;
GAP.FilePath = Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".csv";
GAP.FileName = Path.GetFileName(logfile);
using (PacketExtractor mine = new PacketExtractor())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch (Exception ex)
{
return;
}
mine.logreadmode = true;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
int percent =
(int)
((float)mine.logplaybackfile.BaseStream.Position /
(float)mine.logplaybackfile.BaseStream.Length * 100.0f);
if (progressbar1.Value != percent)
{
progressbar1.Dispatcher.Invoke(() => progressbar1.Value = percent, DispatcherPriority.Background);
}
MAVLinkMessage packet = mine.ReadPacket();
string text = "";
mine.DebugPacket(packet, ref text, false, ",");
Parser.addMessage("* " + mine.lastlogread.ToString("yyyy-MM-dd HH:mm:ss.fff"));
Parser.addMessage(text);
}
Parser.parseMessages();
ArrayList headers = Parser.GetHeaders();
foreach (string header in headers)
{
GlobalAppData.TlogData data = new GlobalAppData.TlogData();
data.HeaderName = header;
data.Name = RM.GetString(header);
GAP.AddTlogData(data);
}
progressbar1.Value = 100;
EnableSettings();
FileLabel.Visibility = Visibility.Visible;
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
}
else if (String.Equals(extension.ToLower(), ".bin"))
{
DialogResult Result = System.Windows.Forms.MessageBox.Show("The .bin file does not display any data parameters." + Environment.NewLine
+ "We will be exporting the .bin file into a .csv file. Are you fine with that?", ".Bin Message", System.Windows.Forms.MessageBoxButtons.YesNo);
if (Result == System.Windows.Forms.DialogResult.Yes)
{
try
{
using (BinaryReader br = new BinaryReader(new BufferedStream(File.OpenRead(logfile), 1024 * 1024)))
{
DateTime displaytimer = DateTime.MinValue;
var length = br.BaseStream.Length;
while (br.BaseStream.Position < length)
{
string data = BL.ReadMessage(br.BaseStream, length);
BP.InputContent(data);
}
}
BP.ParseBIN();
ArrayList OutputMessages = BP.GetMessages();
if (sw != null)
{
sw.Dispose();
}
sw = new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".csv");
foreach (string message in OutputMessages)
{
sw.WriteLine(message);
}
sw.Close();
System.Windows.Forms.MessageBox.Show("You have successfully converted " + Path.GetFileName(logfile) + " into a .csv file");
DisableSettings();
}
catch
{
System.Windows.MessageBox.Show("You currently have the .csv open. Please close your .csv file and retry.", "Open File Error");
}
}
}
else
{
System.Windows.MessageBox.Show("You've selected the wrong file type.", "Wrong File Type Error");
}
}
}
}
}
public MAVLinkMessage ReadPacket(Stream BaseStream)
{
byte[] buffer = new byte[270];
int readcount = 0;
while (readcount < 200)
{
// read STX byte
ReadWithTimeout(BaseStream, buffer, 0, 1);
if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1)
break;
readcount++;
}
var headerlength = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
var headerlengthstx = headerlength + 1;
// read header
ReadWithTimeout(BaseStream, buffer, 1, headerlength);
// packet length
int lengthtoread = 0;
if (buffer[0] == MAVLINK_STX)
{
lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length
if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
{
lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN;
}
}
else
{
lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length
}
//read rest of packet
ReadWithTimeout(BaseStream, buffer, 6, lengthtoread - (headerlengthstx-2));
// resize the packet to the correct length
Array.Resize<byte>(ref buffer, lengthtoread + 2);
MAVLinkMessage message = new MAVLinkMessage(buffer);
// calc crc
ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);
// calc extra bit of crc for mavlink 1.0+
if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1)
{
crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc);
}
// check crc
if ((message.crc16 >> 8) != (crc >> 8) ||
(message.crc16 & 0xff) != (crc & 0xff))
{
badCRC++;
// crc fail
return null;
}
return message;
}
public void addPacket(MAVLinkMessage msg)
{
lock (packetslock)
{
packets[msg.msgid] = msg;
}
}
public object DebugPacket(MAVLinkMessage datin, ref string text)
{
return DebugPacket(datin, ref text, true);
}
/// <summary>
/// Print entire decoded packet to console
/// </summary>
/// <param name="datin">packet byte array</param>
/// <returns>struct of data</returns>
public object DebugPacket(MAVLinkMessage datin, ref string text, bool PrintToConsole, string delimeter = " ")
{
string textoutput = "";
try
{
if (datin.Length > 5)
{
textoutput =
string.Format(
"{0,2:X}{8}{1,2:X}{8}{2,2:X}{8}{3,2:X}{8}{4,2:X}{8}{5,2:X}{8}{6,2:X}{8}{7,6:X}{8}",
datin.header,
datin.payloadlength, datin.incompat_flags, datin.compat_flags, datin.seq, datin.sysid,
datin.compid, datin.msgid, delimeter);
object data = datin.data;
Type test = data.GetType();
if (PrintToConsole)
{
textoutput = textoutput + test.Name + delimeter;
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 + delimeter;
if (fieldValue.GetType() == typeof (byte[]))
{
try
{
byte[] crap = (byte[]) fieldValue;
foreach (byte fiel in crap)
{
if (fiel == 0)
{
break;
}
else
{
textoutput = textoutput + (char) fiel;
}
}
}
catch
{
}
}
if (fieldValue.GetType() == typeof (short[]))
{
try
{
short[] crap = (short[]) fieldValue;
foreach (short fiel in crap)
{
if (fiel == 0)
{
break;
}
else
{
textoutput = textoutput + Convert.ToString(fiel, 16) + "|";
}
}
}
catch
{
}
}
textoutput = textoutput + delimeter;
}
else
{
textoutput = textoutput + field.Name + delimeter + fieldValue.ToString() + delimeter;
}
}
var sig = "";
if (datin.sig != null)
sig = Convert.ToBase64String(datin.sig);
textoutput = textoutput + delimeter + "sig " + sig + delimeter + "Len" + delimeter + datin.Length + "\r\n";
if (PrintToConsole)
Console.Write(textoutput);
if (text != null)
text = textoutput;
}
return data;
}
}
catch
{
textoutput = textoutput + "\r\n";
}
return null;
}
private bool messageReceived(MAVLinkMessage arg)
{
//accept any compid, but filter sysid
if (arg.sysid != _parent.sysid)
{
return(true);
}
if (arg.msgid == (uint)MAVLINK_MSG_ID.DISTANCE_SENSOR)
{
var dist = arg.ToStructure <mavlink_distance_sensor_t>();
if (dist.current_distance >= dist.max_distance)
{
return(true);
}
if (dist.current_distance <= dist.min_distance)
{
return(true);
}
_dS.Add(dist.id, (MAV_SENSOR_ORIENTATION)dist.orientation, dist.current_distance, DateTime.Now, 3);
DataAvailable = true;
}
else if (arg.msgid == (uint)MAVLINK_MSG_ID.OBSTACLE_DISTANCE)
{
var dists = arg.ToStructure <mavlink_obstacle_distance_t>();
var inc = dists.increment == 0 ? dists.increment_f : dists.increment;
var rangestart = dists.angle_offset;
if (dists.frame == (byte)MAV_FRAME.BODY_FRD)
{
// no action
}
else if (dists.frame == (byte)MAV_FRAME.GLOBAL)
{
rangestart += _parent.cs.yaw;
}
var rangeend = rangestart + inc * dists.distances.Length;
for (var a = 0; a < dists.distances.Length; a++)
{
// not used
if (dists.distances[a] == ushort.MaxValue)
{
continue;
}
if (dists.distances[a] > dists.max_distance)
{
continue;
}
if (dists.distances[a] < dists.min_distance)
{
continue;
}
var dist = Math.Min(Math.Max(dists.distances[a], dists.min_distance), dists.max_distance);
var angle = rangestart + inc * a;
_dS.Add(dists.sensor_type, angle, inc, dist, DateTime.Now, 0.2);
}
DataAvailable = true;
}
return(true);
}
private void PacketReceived(MAVLinkMessage buffer)
{
MAVLINK_MSG_ID type = (MAVLINK_MSG_ID) buffer.msgid;
lock (Subscriptions)
{
foreach (var item in Subscriptions)
{
if (item.Key == type)
{
try
{
item.Value(buffer);
}
catch (Exception ex)
{
log.Error(ex);
}
}
}
}
}
public void HandleBuffer(byte[] buffer, int readCount)
{
for (var idx = 0; idx < readCount; ++idx)
{
var value = (byte)buffer[idx];
if (_currentMessage != null)
{
_currentMessage.add_byte(value);
}
switch (_mavLinkParserState)
{
case MavLinkParserStates.ExpectingStx:
if (value == MAVLINK2_STX)
{
_currentMessage = new MAVLinkMessage();
_currentMessage.add_byte(value);
_mavLinkParserState = MavLinkParserStates.Expecting2PayloadLen;
}
if (value == 254)
{
_currentMessage = new MAVLinkMessage();
_currentMessage.add_byte(value);
_mavLinkParserState = MavLinkParserStates.ExpectingPayloadLen;
}
break;
case MavLinkParserStates.Expecting2PayloadLen:
_currentMessage.payloadlength = value;
_mavLinkParserState = MavLinkParserStates.Expecting2IncompatFlags;
break;
case MavLinkParserStates.Expecting2IncompatFlags:
_currentMessage.incompat_flags = value;
_mavLinkParserState = MavLinkParserStates.Expecting2CompatFlag;
break;
case MavLinkParserStates.Expecting2CompatFlag:
_currentMessage.compat_flags = value;
_mavLinkParserState = MavLinkParserStates.Expecting2Seq;
break;
case MavLinkParserStates.Expecting2Seq:
_currentMessage.seq = value;
_mavLinkParserState = MavLinkParserStates.Expecting2SysId;
break;
case MavLinkParserStates.Expecting2SysId:
_currentMessage.sysid = value;
_mavLinkParserState = MavLinkParserStates.Expecting2ComponentId;
break;
case MavLinkParserStates.Expecting2ComponentId:
_currentMessage.compid = value;
_mavLinkParserState = MavLinkParserStates.Expecting2MsgId07;
_currentMessage.msgid = 0x00;
break;
case MavLinkParserStates.Expecting2MsgId07:
_currentMessage.msgid = value;
_mavLinkParserState = MavLinkParserStates.Expecting2MsgId815;
break;
case MavLinkParserStates.Expecting2MsgId815:
_mavLinkParserState = MavLinkParserStates.Expecting2MsgId1623;
_currentMessage.msgid += (UInt32)(value << 8);
break;
case MavLinkParserStates.Expecting2MsgId1623:
_mavLinkParserState = MavLinkParserStates.Expecting2TargetSysId;
_currentMessage.msgid += (UInt32)(value << 16);
break;
case MavLinkParserStates.Expecting2TargetSysId:
_currentMessage.targetsysid = value;
_mavLinkParserState = MavLinkParserStates.Expecting2TargetComponentId;
break;
case MavLinkParserStates.Expecting2TargetComponentId:
_currentMessage.targetcomponentid = value;
_currentMessage.payload_index = 0;
_mavLinkParserState = MavLinkParserStates.ReadingPayload2;
break;
case MavLinkParserStates.ReadingPayload2:
if (_currentMessage.payload_index < _currentMessage.payloadlength)
{
_currentMessage.payload[_currentMessage.payload_index++] = value;
}
if (_currentMessage.payload_index == _currentMessage.payloadlength)
{
_mavLinkParserState = MavLinkParserStates.Expecting2CheckSum1;
}
break;
case MavLinkParserStates.Expecting2CheckSum1:
_currentMessage.crc16 = value;
_mavLinkParserState = MavLinkParserStates.Expecting2CheckSum2;
break;
case MavLinkParserStates.Expecting2CheckSum2:
_currentMessage.crc16 += (UInt16)(value << 8);
_mavLinkParserState = MavLinkParserStates.Expecting2Signature;
_currentMessage.sig = new byte[13];
_signatureIndex = 0;
break;
case MavLinkParserStates.Expecting2Signature:
_currentMessage.sig[_signatureIndex++] = value;
if (_signatureIndex == SIGNATURE_LEN)
{
Debug.WriteLine($"MAVLINK2 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16}");
_currentMessage.processBuffer(_currentMessage.buffer);
Debug.WriteLine($"MAVLINK2 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16}");
_mavLinkParserState = MavLinkParserStates.ExpectingStx;
_currentMessage = null;
}
break;
case MavLinkParserStates.ExpectingPayloadLen:
_currentMessage.payloadlength = value;
_mavLinkParserState = MavLinkParserStates.ExpectingSeqNumber;
break;
case MavLinkParserStates.ExpectingSeqNumber:
_currentMessage.seq = value;
_mavLinkParserState = MavLinkParserStates.ExpectingSystemId;
break;
case MavLinkParserStates.ExpectingSystemId:
_currentMessage.sysid = value;
_mavLinkParserState = MavLinkParserStates.ExpectingComponentId;
break;
case MavLinkParserStates.ExpectingComponentId:
_currentMessage.compid = value;
_mavLinkParserState = MavLinkParserStates.ExpectingMessageId;
break;
case MavLinkParserStates.ExpectingMessageId:
_currentMessage.msgid = value;
_mavLinkParserState = MavLinkParserStates.ReadingPayloadData;
break;
case MavLinkParserStates.ReadingPayloadData:
if (_currentMessage.payload_index < _currentMessage.payloadlength)
{
_currentMessage.payload[_currentMessage.payload_index++] = value;
}
if (_currentMessage.payload_index == _currentMessage.payloadlength)
{
_currentMessage.MessageInfo = MAVLink.MAVLINK_MESSAGE_INFOS.GetMessageInfo(_currentMessage.msgid);
_currentMessage.crc16Calc = MavlinkCRC.crc_calculate(_currentMessage.buffer, _currentMessage.buffer_index);
_currentMessage.crc16Calc = MavlinkCRC.crc_accumulate(_currentMessage.MessageInfo.crc, _currentMessage.crc16Calc);
_mavLinkParserState = MavLinkParserStates.ExpectingCRC1;
}
break;
case MavLinkParserStates.ExpectingCRC1:
_currentMessage.crc16 = value;
_mavLinkParserState = MavLinkParserStates.ExpectingCRC2;
break;
case MavLinkParserStates.ExpectingCRC2:
_currentMessage.crc16 += (UInt16)(value << 8);
if ((_currentMessage.crc16 >> 8) != (_currentMessage.crc16Calc >> 8) || (_currentMessage.crc16 & 0xff) != (_currentMessage.crc16Calc & 0xff))
{
//Debug.WriteLine($"INVALID CHECK SUM MAVLINK1 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16:x2} - {_currentMessage.crc16Calc:x2}");
}
else
{
MessageParsed?.Invoke(this, _currentMessage);
///Debug.WriteLine($"VALID CHECK SUM MAVLINK1 - Message Id: {_currentMessage.msgid} - {_currentMessage.payloadlength} - {_currentMessage.seq} - {_currentMessage.crc16:x2} - {_currentMessage.crc16Calc:x2}");
}
_currentMessage.processBuffer(_currentMessage.buffer);
_currentMessage = null;
_mavLinkParserState = MavLinkParserStates.ExpectingStx;
break;
}
}
}
private void OpenBg(object PRsender, bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
{
frmProgressReporter.UpdateProgressAndStatus(-1, Strings.MavlinkConnecting);
giveComport = true;
if (BaseStream is SerialPort)
{
// allow settings to settle - previous dtr
System.Threading.Thread.Sleep(1000);
}
Terrain = new TerrainFollow(this);
bool hbseen = false;
try
{
BaseStream.ReadBufferSize = 16*1024;
lock (objlock) // so we dont have random traffic
{
log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate);
BaseStream.Open();
BaseStream.DiscardInBuffer();
// other boards seem to have issues if there is no delay? posible bootloader timeout issue
Thread.Sleep(1000);
}
MAVLinkMessage buffer = new MAVLinkMessage();
MAVLinkMessage buffer1 = new MAVLinkMessage();
DateTime start = DateTime.Now;
DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS);
var countDown = new System.Timers.Timer {Interval = 1000, AutoReset = false};
countDown.Elapsed += (sender, e) =>
{
int secondsRemaining = (deadline - e.SignalTime).Seconds;
frmProgressReporter.UpdateProgressAndStatus(-1, string.Format(Strings.Trying, secondsRemaining));
if (secondsRemaining > 0) countDown.Start();
};
countDown.Start();
// px4 native
BaseStream.WriteLine("sh /etc/init.d/rc.usb");
int count = 0;
while (true)
{
if (progressWorkerEventArgs.CancelRequested)
{
progressWorkerEventArgs.CancelAcknowledged = true;
countDown.Stop();
if (BaseStream.IsOpen)
BaseStream.Close();
giveComport = false;
return;
}
log.Info(DateTime.Now.Millisecond + " Start connect loop ");
if (DateTime.Now > deadline)
{
//if (Progress != null)
// Progress(-1, "No Heartbeat Packets");
countDown.Stop();
this.Close();
if (hbseen)
{
progressWorkerEventArgs.ErrorMessage = Strings.Only1Hb;
throw new Exception(Strings.Only1HbD);
}
else
{
progressWorkerEventArgs.ErrorMessage = "No Heartbeat Packets Received";
throw new Exception(@"Can not establish a connection\n
Please check the following
1. You have firmware loaded
2. You have the correct serial port selected
3. PX4 - You have the microsd card installed
4. Try a diffrent usb port\n\n" +
"No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nMission Planner waits for 2 valid heartbeat packets before connecting");
}
}
System.Threading.Thread.Sleep(1);
// can see 2 heartbeat packets at any time, and will connect - was one after the other
if (buffer.Length == 0)
buffer = getHeartBeat();
System.Threading.Thread.Sleep(1);
if (buffer1.Length == 0)
buffer1 = getHeartBeat();
if (buffer.Length > 0 || buffer1.Length > 0)
hbseen = true;
count++;
// 2 hbs that match
if (buffer.Length > 5 && buffer1.Length > 5 && buffer.sysid == buffer1.sysid && buffer.compid == buffer1.compid)
{
mavlink_heartbeat_t hb = buffer.ToStructure<mavlink_heartbeat_t>();
if (hb.type != (byte) MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer, hb);
break;
}
}
// 2 hb's that dont match. more than one sysid here
if (buffer.Length > 5 && buffer1.Length > 5 && (buffer.sysid == buffer1.sysid || buffer.compid == buffer1.compid))
{
mavlink_heartbeat_t hb = buffer.ToStructure<mavlink_heartbeat_t>();
if (hb.type != (byte) MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte) MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer, hb);
break;
}
hb = buffer1.ToStructure<mavlink_heartbeat_t>();
if (hb.type != (byte) MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte) MAVLink.MAV_TYPE.GCS)
{
SetupMavConnect(buffer1, hb);
break;
}
}
}
countDown.Stop();
getVersion();
frmProgressReporter.UpdateProgressAndStatus(0,
"Getting Params.. (sysid " + MAV.sysid + " compid " + MAV.compid + ") ");
if (getparams)
{
getParamListBG();
}
byte[] temp = ASCIIEncoding.ASCII.GetBytes("Mission Planner " + Application.ProductVersion + "\0");
Array.Resize(ref temp, 50);
generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT,
new mavlink_statustext_t() {severity = (byte) MAV_SEVERITY.INFO, text = temp});
if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true)
{
giveComport = false;
if (BaseStream.IsOpen)
BaseStream.Close();
return;
}
}
catch (Exception e)
{
try
{
BaseStream.Close();
}
catch
{
}
giveComport = false;
if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage))
progressWorkerEventArgs.ErrorMessage = Strings.ConnectFailed;
log.Error(e);
throw;
}
//frmProgressReporter.Close();
giveComport = false;
frmProgressReporter.UpdateProgressAndStatus(100, Strings.Done);
log.Info("Done open " + MAV.sysid + " " + MAV.compid);
MAV.packetslost = 0;
MAV.synclost = 0;
}
private void _link_MessageParsed(object sender, MAVLinkMessage e)
{
}
void SetupMavConnect(MAVLinkMessage message, mavlink_heartbeat_t hb)
{
sysidcurrent = message.sysid;
compidcurrent = message.compid;
mavlinkversion = hb.mavlink_version;
MAV.aptype = (MAV_TYPE) hb.type;
MAV.apname = (MAV_AUTOPILOT) hb.autopilot;
setAPType(message.sysid, message.compid);
MAV.sysid = message.sysid;
MAV.compid = message.compid;
MAV.recvpacketcount = message.seq;
log.InfoFormat("ID sys {0} comp {1} ver{2} type {3} name {4}", MAV.sysid, MAV.compid, mavlinkversion,
MAV.aptype.ToString(), MAV.apname.ToString());
}
private void _telemeteryLink_MessageParsed(object sender, MAVLinkMessage msg)
{
_droneAdapter.UpdateDroneAsync(_apmDrone, msg);
}
