| public ReadPacket ( Stream BaseStream ) : MAVLinkMessage | ||
| BaseStream | Stream | |
| 리턴 | MAVLinkMessage | |
public static void anonymise(string logfile, string outputfile)
{
if (!File.Exists(logfile))
{
return;
}
var rand = new Random();
var random = rand.NextDouble() * (rand.NextDouble() * 3);
// TLOG
if (logfile.ToLower().EndsWith(".tlog"))
{
Comms.CommsFile tlogFile = new CommsFile();
tlogFile.Open(logfile);
using (var stream = new CommsStream(tlogFile, tlogFile.BytesToRead))
using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read))
{
MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse(true);
var block = new Type[]
{
typeof(MAVLink.mavlink_fence_point_t),
typeof(MAVLink.mavlink_simstate_t),
typeof(MAVLink.mavlink_rally_point_t),
typeof(MAVLink.mavlink_ahrs2_t),
typeof(MAVLink.mavlink_camera_feedback_t),
typeof(MAVLink.mavlink_ahrs3_t),
typeof(MAVLink.mavlink_deepstall_t),
typeof(MAVLink.mavlink_gps_raw_int_t),
typeof(MAVLink.mavlink_global_position_int_t),
typeof(MAVLink.mavlink_set_gps_global_origin_t),
typeof(MAVLink.mavlink_gps_global_origin_t),
typeof(MAVLink.mavlink_global_position_int_cov_t),
typeof(MAVLink.mavlink_set_position_target_global_int_t),
typeof(MAVLink.mavlink_hil_state_t),
typeof(MAVLink.mavlink_sim_state_t),
typeof(MAVLink.mavlink_hil_gps_t),
typeof(MAVLink.mavlink_hil_state_quaternion_t),
typeof(MAVLink.mavlink_gps2_raw_t),
typeof(MAVLink.mavlink_terrain_request_t),
typeof(MAVLink.mavlink_terrain_check_t),
typeof(MAVLink.mavlink_terrain_report_t),
typeof(MAVLink.mavlink_follow_target_t),
typeof(MAVLink.mavlink_gps_input_t),
typeof(MAVLink.mavlink_high_latency_t),
typeof(MAVLink.mavlink_home_position_t),
typeof(MAVLink.mavlink_set_home_position_t),
typeof(MAVLink.mavlink_adsb_vehicle_t),
typeof(MAVLink.mavlink_camera_image_captured_t),
typeof(MAVLink.mavlink_uavionix_adsb_out_dynamic_t),
typeof(MAVLink.mavlink_global_position_int_t),
typeof(MAVLink.mavlink_set_home_position_t),
typeof(MAVLink.mavlink_home_position_t),
typeof(MAVLink.mavlink_set_position_target_global_int_t),
typeof(MAVLink.mavlink_local_position_ned_t),
typeof(MAVLink.mavlink_command_long_t),
typeof(MAVLink.mavlink_mission_item_t),
typeof(MAVLink.mavlink_mission_item_int_t),
typeof(MAVLink.mavlink_uavionix_adsb_out_cfg_t)
};
var checks = new string[]
{
"lat", "latitude", "lat_int", "landing_lat",
"path_lat", "arc_entry_lat", "gpsLat", "gpsOffsetLat",
"lon", "longitude", "lon_int", "landing_lon",
"path_lon", "arc_entry_lon", "gpsLon", "gpsOffsetLon"
};
while (stream.Position < stream.Length)
{
var packet = parse.ReadPacket(stream);
if (packet == null)
{
continue;
}
var msginfo = MAVLink.MAVLINK_MESSAGE_INFOS.GetMessageInfo(packet.msgid);
if (block.Contains(msginfo.type))
{
bool valid = false;
var oldrxtime = packet.rxtime;
foreach (var check in checks)
{
var field = msginfo.type.GetField(check);
if (field != null)
{
var pkt = packet.data;
var value = field.GetValue(pkt);
if (value is Int32)
{
if ((int)value != 0)
{
field.SetValue(pkt, (int)((int)value + random * 1e7));
}
packet = new MAVLink.MAVLinkMessage(
parse.GenerateMAVLinkPacket20((MAVLink.MAVLINK_MSG_ID)msginfo.msgid,
pkt, false, packet.sysid, packet.compid, packet.seq));
valid = true;
}
else if (value is Single)
{
if ((Single)value != 0)
{
field.SetValue(pkt, (Single)value + random);
}
packet = new MAVLink.MAVLinkMessage(
parse.GenerateMAVLinkPacket20((MAVLink.MAVLINK_MSG_ID)msginfo.msgid,
pkt, false, packet.sysid, packet.compid, packet.seq));
valid = true;
}
else
{
continue;
}
}
}
packet.rxtime = oldrxtime;
}
byte[] datearray =
BitConverter.GetBytes(
(UInt64)((packet.rxtime.ToUniversalTime() - new DateTime(1970, 1, 1))
.TotalMilliseconds * 1000));
Array.Reverse(datearray);
outfilestream.Write(datearray, 0, datearray.Length);
outfilestream.Write(packet.buffer, 0, packet.Length);
}
}
}
else //LOG
{
using (DFLogBuffer col = new DFLogBuffer(File.OpenRead(logfile)))
using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read))
{
foreach (var dfItem in col.GetEnumeratorTypeAll())
{
var index = col.dflog.FindMessageOffset(dfItem.msgtype, "Lat");
var index2 = col.dflog.FindMessageOffset(dfItem.msgtype, "Lng");
if (index != -1)
{
var lat = Double.Parse(dfItem.items[index], CultureInfo.InvariantCulture);
if (lat != 0)
{
dfItem.items[index] = (lat + random).ToString(CultureInfo.InvariantCulture);
}
}
if (index2 != -1)
{
var lon = Double.Parse(dfItem.items[index2], CultureInfo.InvariantCulture);
if (lon != 0)
{
dfItem.items[index2] = (lon + random).ToString(CultureInfo.InvariantCulture);
}
}
var str = String.Join(",", dfItem.items) + "\r\n";
outfilestream.Write(ASCIIEncoding.ASCII.GetBytes(str), 0, str.Length);
}
}
}
}
private static void mainloop()
{
run = true;
byte[] buffer = new byte[1024];
MAVLink.MavlinkParse mav = new MAVLink.MavlinkParse();
while (run)
{
try
{
while (Server.Connected && Server.Available > 0)
{
int read = Server.GetStream().Read(buffer, 0, buffer.Length);
// write to all clients
foreach (var client in clients)
{
if (client.Connected)
client.GetStream().Write(buffer, 0, read);
}
}
}
catch
{
}
// read from all clients
foreach (var client in clients)
{
try
{
while (client.Connected && client.Available > 0)
{
byte[] packet = mav.ReadPacket(client.GetStream());
if (packet == null)
continue;
if (Server != null && Server.Connected)
Server.GetStream().Write(packet, 0, packet.Length);
}
}
catch
{
client.Close();
}
}
System.Threading.Thread.Sleep(1);
}
}
public static void ProcessFile(string logfile)
{
if (File.Exists(logfile + ".jpg"))
{
return;
}
double minx = 99999;
double maxx = -99999;
double miny = 99999;
double maxy = -99999;
bool sitl = false;
Dictionary <int, List <PointLatLngAlt> > loc_list = new Dictionary <int, List <PointLatLngAlt> >();
try
{
if (logfile.ToLower().EndsWith(".tlog"))
{
Comms.CommsFile cf = new CommsFile();
cf.Open(logfile);
using (CommsStream cs = new CommsStream(cf, cf.BytesToRead))
{
MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse(true);
while (cs.Position < cs.Length)
{
try
{
MAVLink.MAVLinkMessage packet = parse.ReadPacket(cs);
if (packet == null || packet.Length < 5)
{
continue;
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.SIM_STATE ||
packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.SIMSTATE)
{
sitl = true;
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT)
{
var loc = packet.ToStructure <MAVLink.mavlink_global_position_int_t>();
if (loc.lat == 0 || loc.lon == 0)
{
continue;
}
var id = packet.sysid * 256 + packet.compid;
if (!loc_list.ContainsKey(id))
{
loc_list[id] = new List <PointLatLngAlt>();
}
loc_list[id].Add(new PointLatLngAlt(loc.lat / 10000000.0f, loc.lon / 10000000.0f));
minx = Math.Min(minx, loc.lon / 10000000.0f);
maxx = Math.Max(maxx, loc.lon / 10000000.0f);
miny = Math.Min(miny, loc.lat / 10000000.0f);
maxy = Math.Max(maxy, loc.lat / 10000000.0f);
}
}
catch { }
}
}
cf.Close();
}
else if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log"))
{
using (
CollectionBuffer colbuf =
new CollectionBuffer(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite))
)
{
loc_list[0] = new List <PointLatLngAlt>();
foreach (var item in colbuf.GetEnumeratorType("GPS"))
{
if (item.msgtype.StartsWith("GPS"))
{
if (!colbuf.dflog.logformat.ContainsKey("GPS"))
{
continue;
}
var status =
double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Status")]);
var lat = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Lat")]);
var lon = double.Parse(item.items[colbuf.dflog.FindMessageOffset(item.msgtype, "Lng")]);
if (lat == 0 || lon == 0 || status < 3)
{
continue;
}
loc_list[0].Add(new PointLatLngAlt(lat, lon));
minx = Math.Min(minx, lon);
maxx = Math.Max(maxx, lon);
miny = Math.Min(miny, lat);
maxy = Math.Max(maxy, lat);
}
}
}
}
if (loc_list.Count > 0 && loc_list.First().Value.Count > 10)
{
// add a bit of buffer
var area = RectLatLng.FromLTRB(minx - 0.001, maxy + 0.001, maxx + 0.001, miny - 0.001);
using (var map = GetMap(area))
using (var grap = Graphics.FromImage(map))
{
if (sitl)
{
AddTextToMap(grap, "SITL");
}
Color[] colours =
{
Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo,
Color.Violet, Color.Pink
};
int a = 0;
foreach (var locs in loc_list.Values)
{
PointF lastpoint = new PointF();
var pen = new Pen(colours[a % (colours.Length - 1)]);
foreach (var loc in locs)
{
PointF newpoint = GetPixel(area, loc, map.Size);
if (!lastpoint.IsEmpty)
{
grap.DrawLine(pen, lastpoint, newpoint);
}
lastpoint = newpoint;
}
a++;
}
map.Save(logfile + ".jpg", SKEncodedImageFormat.Jpeg);
File.SetLastWriteTime(logfile + ".jpg", new FileInfo(logfile).LastWriteTime);
}
}
else
{
DoTextMap(logfile + ".jpg", "No gps data");
File.SetLastWriteTime(logfile + ".jpg", new FileInfo(logfile).LastWriteTime);
}
}
catch (Exception ex)
{
if (ex.ToString().Contains("Mavlink 0.9"))
{
DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9");
}
}
}
void bgw_DoWork(object sender, DoWorkEventArgs e)
{
while (serialPort1.IsOpen)
{
try
{
MAVLink.MAVLinkMessage packet;
lock (readlock)
{
// read any valid packet from the port
packet = mavlink.ReadPacket(serialPort1.BaseStream);
// check its valid
if (packet == null || packet.data == null)
{
continue;
}
}
// check to see if its a hb packet from the comport
if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
// save the sysid and compid of the seen MAV
sysid = packet.sysid;
compid = packet.compid;
// request streams at 2 hz
var buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
new MAVLink.mavlink_request_data_stream_t()
{
req_message_rate = 2,
req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL,
start_stop = 1,
target_component = compid,
target_system = sysid
});
serialPort1.Write(buffer, 0, buffer.Length);
buffer = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);
serialPort1.Write(buffer, 0, buffer.Length);
}
// from here we should check the the message is addressed to us
if (sysid != packet.sysid || compid != packet.compid)
{
continue;
}
Console.WriteLine(packet.msgtypename);
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
//or
//if (packet.data.GetType() == typeof(MAVLink.mavlink_attitude_t))
{
var att = (MAVLink.mavlink_attitude_t)packet.data;
Console.WriteLine(att.pitch * 57.2958 + " " + att.roll * 57.2958);
}
}
catch
{
}
System.Threading.Thread.Sleep(1);
}
}
private static void mainloop()
{
run = true;
byte[] buffer = new byte[1024];
MAVLink.MavlinkParse mav = new MAVLink.MavlinkParse();
while (run)
{
try
{
if (Server == null)
{
System.Threading.Thread.Sleep(1);
continue;
}
while (Server.Connected && Server.Available > 0)
{
int read = Server.GetStream().Read(buffer, 0, buffer.Length);
// write to all clients
foreach (var client in clients.ToArray())
{
if (client.Connected)
{
client.GetStream().Write(buffer, 0, read);
}
}
}
}
catch
{
}
// read from all clients
foreach (var client in clients.ToArray())
{
try
{
while (client.Connected && client.Available > 0)
{
var packet = mav.ReadPacket(client.GetStream());
if (packet == null)
{
continue;
}
if (Server != null && Server.Connected)
{
Server.GetStream().Write(packet.buffer, 0, packet.Length);
}
}
}
catch
{
client.Close();
}
}
System.Threading.Thread.Sleep(1);
}
}
public static void Start()
{
var config = Settings.Instance[SettingsName];
if (config == null)
{
Settings.Instance[SettingsName] = connectionInfos.ToJSON();
config = Settings.Instance[SettingsName];
}
connectionInfos = config.FromJSON <List <ConnectionInfo> >();
foreach (var connectionInfo in connectionInfos)
{
if (connectionInfo.Enabled == false)
{
continue;
}
log.Info(connectionInfo.ToJSON());
if (connectionInfo.Format == ConnectionFormat.MAVLink)
{
if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Inbound)
{
try
{
var client = new UdpClient(connectionInfo.Port);
client.BeginReceive(clientdataMAVLink, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Outbound)
{
try
{
// create and set default dest
var client = new UdpClient(connectionInfo.ConfigString, connectionInfo.Port);
client.SendAsync(new byte[] { 0 }, 1);
client.BeginReceive(clientdataMAVLink, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Tcp &&
connectionInfo.Direction == Direction.Outbound)
{
try
{
// try anything already connected
Task.Run(() =>
{
try
{
var serial = new TcpSerial();
serial.Host = connectionInfo.ConfigString;
serial.Port = connectionInfo.Port.ToString();
serial.ReadBufferSize = 1024 * 300;
serial.Open();
// sample 1.2seconds
Thread.Sleep(1200);
var btr = serial.BytesToRead;
var buffer = new byte[btr];
serial.Read(buffer, 0, buffer.Length);
//serial.Close();
var parse = new MAVLink.MavlinkParse();
var st = buffer.ToMemoryStream();
while (st.Position < st.Length)
{
var packet = parse.ReadPacket(st);
if (packet != null)
{
if (packet.msgid == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
NewMavlinkConnection?.BeginInvoke(null, serial, null, null);
return;
}
}
}
}
catch
{
}
});
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Tcp &&
connectionInfo.Direction == Direction.Inbound)
{
try
{
// try anything already connected
Task.Run(() =>
{
try
{
TcpListener listener = new TcpListener(IPAddress.Any, connectionInfo.Port);
listener.Start();
var client = listener.AcceptTcpClient();
var serial = new TcpSerial();
serial.client = client;
serial.ReadBufferSize = 1024 * 300;
serial.Open();
// sample 1.2seconds
Thread.Sleep(1200);
var btr = serial.BytesToRead;
var buffer = new byte[btr];
serial.Read(buffer, 0, buffer.Length);
//serial.Close();
var parse = new MAVLink.MavlinkParse();
var st = buffer.ToMemoryStream();
while (st.Position < st.Length)
{
var packet = parse.ReadPacket(st);
if (packet != null)
{
if (packet.msgid == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
NewMavlinkConnection?.BeginInvoke(null, serial, null, null);
return;
}
}
}
}
catch
{
}
});
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Serial &&
connectionInfo.Direction == Direction.Outbound)
{
try
{
// try anything already connected
Task.Run(() =>
{
Parallel.ForEach(SerialPort.GetPortNames(), port =>
{
try
{
var serial = new SerialPort(port, connectionInfo.Port);
serial.ReadBufferSize = 1024 * 300;
serial.Open();
// sample 1.2seconds
Thread.Sleep(1200);
var btr = serial.BytesToRead;
var buffer = new byte[btr];
serial.Read(buffer, 0, buffer.Length);
serial.Close();
var parse = new MAVLink.MavlinkParse();
var st = buffer.ToMemoryStream();
while (st.Position < st.Length)
{
var packet = parse.ReadPacket(st);
if (packet != null)
{
if (packet.msgid == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
NewMavlinkConnection?.BeginInvoke(null, serial, null, null);
return;
}
}
}
}
catch
{
}
});
});
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
}
else if (connectionInfo.Format == ConnectionFormat.Video)
{
if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Inbound)
{
try
{
var client = new UdpClient(connectionInfo.Port, AddressFamily.InterNetwork);
client.BeginReceive(clientdataVideo, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Tcp && connectionInfo.Direction == Direction.Inbound)
{
try
{
var client = new TcpListener(IPAddress.Any, connectionInfo.Port);
client.Start();
client.BeginAcceptTcpClient(clientdatatcpvideo, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Direction == Direction.Outbound)
{
NewVideoStream?.BeginInvoke(null,
connectionInfos.First(a => a == connectionInfo).ConfigString, null, null);
continue;
}
}
}
}
static void doread(object o)
{
lock (runlock)
{
running++;
}
var portname = (string)o;
Console.WriteLine("Scanning {0}", portname);
try
{
using (SerialPort port = new SerialPort())
{
port.PortName = portname;
foreach (var baud in bauds)
{
// try default baud
if (baud != 0)
{
port.BaudRate = baud;
}
port.Open();
port.DiscardInBuffer();
// let data flow in
Thread.Sleep(2000);
lock (runlock)
{
if (run == 0)
{
return;
}
}
int available = port.BytesToRead;
var buffer = new byte[available];
int read = port.Read(buffer, 0, available);
Console.WriteLine("{0} {1}", portname, read);
if (read > 0)
{
using (MemoryStream ms = new MemoryStream(buffer, 0, read))
{
MAVLink.MavlinkParse mav = new MAVLink.MavlinkParse();
try
{
again:
var packet = mav.ReadPacket(ms);
if (packet != null && packet.Length > 0)
{
port.Close();
Console.WriteLine("Found Mavlink on port {0} at {1}", port.PortName,
port.BaudRate);
foundport = true;
portinterface = port;
if (connect)
{
MainV2.comPort.BaseStream = port;
doconnect();
}
break;
}
Console.WriteLine(portname + " crc: " + mav.badCRC);
goto again;
}
catch (Exception ex)
{
Console.WriteLine(portname + " " + ex.ToString());
}
}
}
try
{
port.Close();
}
catch (Exception ex)
{
Console.WriteLine(ex.ToString());
}
if (foundport)
{
break;
}
}
}
}
catch (Exception ex)
{
Console.WriteLine(portname + " " + ex.ToString());
}
finally
{
lock (runlock)
{
running--;
if (running == 0)
{
run = 0;
}
}
}
Console.WriteLine("Scan port {0} Finished!!", portname);
}
public static void tlog(string logfile)
{
List <MLArray> mlList = new List <MLArray>();
Hashtable datappl = new Hashtable();
using (Comms.CommsFile cf = new CommsFile(logfile))
using (CommsStream cs = new CommsStream(cf, cf.BytesToRead))
{
MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse(true);
while (cs.Position < cs.Length)
{
MAVLink.MAVLinkMessage packet = parse.ReadPacket(cs);
object data = packet.data;
if (data == null)
{
continue;
}
if (data is MAVLink.mavlink_heartbeat_t)
{
if (((MAVLink.mavlink_heartbeat_t)data).type == (byte)MAVLink.MAV_TYPE.GCS)
{
continue;
}
}
Type test = data.GetType();
DateTime time = packet.rxtime;
double matlabtime = GetMatLabSerialDate(time);
try
{
foreach (var field in test.GetFields())
{
// field.Name has the field's name.
object fieldValue = field.GetValue(data); // Get value
if (field.FieldType.IsArray)
{
}
else
{
if (!datappl.ContainsKey(field.Name + "_" + field.DeclaringType.Name))
{
datappl[field.Name + "_" + field.DeclaringType.Name] = new List <double[]>();
}
List <double[]> list =
((List <double[]>)datappl[field.Name + "_" + field.DeclaringType.Name]);
object value = fieldValue;
if (value.GetType() == typeof(Single))
{
list.Add(new double[] { matlabtime, (double)(Single)field.GetValue(data) });
}
else if (value.GetType() == typeof(short))
{
list.Add(new double[] { matlabtime, (double)(short)field.GetValue(data) });
}
else if (value.GetType() == typeof(ushort))
{
list.Add(new double[] { matlabtime, (double)(ushort)field.GetValue(data) });
}
else if (value.GetType() == typeof(byte))
{
list.Add(new double[] { matlabtime, (double)(byte)field.GetValue(data) });
}
else if (value.GetType() == typeof(sbyte))
{
list.Add(new double[] { matlabtime, (double)(sbyte)field.GetValue(data) });
}
else if (value.GetType() == typeof(Int32))
{
list.Add(new double[] { matlabtime, (double)(Int32)field.GetValue(data) });
}
else if (value.GetType() == typeof(UInt32))
{
list.Add(new double[] { matlabtime, (double)(UInt32)field.GetValue(data) });
}
else if (value.GetType() == typeof(ulong))
{
list.Add(new double[] { matlabtime, (double)(ulong)field.GetValue(data) });
}
else if (value.GetType() == typeof(long))
{
list.Add(new double[] { matlabtime, (double)(long)field.GetValue(data) });
}
else if (value.GetType() == typeof(double))
{
list.Add(new double[] { matlabtime, (double)(double)field.GetValue(data) });
}
else
{
Console.WriteLine("Unknown data type");
}
}
}
}
catch
{
}
}
}
foreach (string item in datappl.Keys)
{
double[][] temp = ((List <double[]>)datappl[item]).ToArray();
MLArray dbarray = new MLDouble(item.Replace(" ", "_"), temp);
mlList.Add(dbarray);
}
try
{
MatFileWriter mfw = new MatFileWriter(logfile + ".mat", mlList, false);
}
catch (Exception err)
{
throw new Exception("There was an error when creating the MAT-file: \n" + err.ToString(), err);
}
}
void bgw_DoWork(object sender, DoWorkEventArgs e)
{
while (serialPort1.IsOpen)
{
try
{
MAVLink.MAVLinkMessage packet;
lock (readlock)
{
// read any valid packet from the port
// 从端口读取任何有效的数据包
packet = mavlink.ReadPacket(serialPort1.BaseStream);
// check its valid
// 检查它的有效
if (packet == null || packet.data == null)
{
continue;
}
}
// check to see if its a hb packet from the comport
// 看看它的心跳包从表现
if (packet.data.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
// save the sysid and compid of the seen MAV
// 保存系统ID和目标飞行器的计算机ID
sysid = packet.sysid;
compid = packet.compid;
// request streams at 2 hz
// 请求的数据流在2Hz
mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
new MAVLink.mavlink_request_data_stream_t()
{
req_message_rate = 2,
req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL,
start_stop = 1,
target_component = compid,
target_system = sysid
});
// 设置mavlink数据缓冲区格式
}
// from here we should check the the message is addressed to us
// 从这里我们应该检查的消息是写给我们（直）
if (sysid != packet.sysid || compid != packet.compid)
{
continue;
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE)
//or
//或
//if (packet.data.GetType() == typeof(MAVLink.mavlink_attitude_t))
{
//var att = (MAVLink.mavlink_attitude_t)packet.data;// 读取一帧姿态包
//Console.WriteLine(att.pitch*57.2958 + " " + att.roll*57.2958);
var imm = (MAVLink.mavlink_attitude_t)packet.data;
Console.WriteLine("Roll:" + imm.roll + ";Pitch:" + imm.pitch + ";Yaw:" + imm.yaw + ";");
}
}
catch
{
}
System.Threading.Thread.Sleep(1);
}
}
public static void anonymise(string logfile, string outputfile)
{
var latrandom = new Random().NextDouble();
//LOG
using (CollectionBuffer col = new CollectionBuffer(File.OpenRead(logfile)))
using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read))
{
foreach (var dfItem in col.GetEnumeratorTypeAll())
{
var index = col.dflog.FindMessageOffset(dfItem.msgtype, "lat");
if (index != -1)
{
dfItem.items[index] =
(Double.Parse(dfItem.items[index], CultureInfo.InvariantCulture) + latrandom).ToString(
CultureInfo.InvariantCulture);
}
var str = String.Join(",", dfItem.items) + "\r\n";
outfilestream.Write(ASCIIEncoding.ASCII.GetBytes(str), 0, str.Length);
}
}
return;
// TLOG
Comms.CommsFile tlogFile = new CommsFile();
tlogFile.Open(logfile);
using (var stream = new CommsStream(tlogFile, tlogFile.BytesToRead))
using (var outfilestream = File.Open(outputfile, FileMode.Create, FileAccess.ReadWrite, FileShare.Read))
{
MAVLink.MavlinkParse parse = new MAVLink.MavlinkParse();
double lat = 0;
double lng = 0;
while (stream.Position < stream.Length)
{
var packet = parse.ReadPacket(stream);
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT)
{
packet.ToStructure <MAVLink.mavlink_global_position_int_t>();
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.POSITION_TARGET_GLOBAL_INT)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.SET_HOME_POSITION)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.HOME_POSITION)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.LOCAL_POSITION_NED)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM)
{
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_INT)
{
}
byte[] datearray =
BitConverter.GetBytes(
(UInt64)((DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds * 1000));
Array.Reverse(datearray);
outfilestream.Write(datearray, 0, datearray.Length);
outfilestream.Write(packet.buffer, 0, packet.Length);
}
}
}
// Update is called once per frame
void Update()
{
if (sock.Available > 0)
{
int recvBytes = 0;
try
{
recvBytes = sock.Receive(buf);
}
catch (SocketException e)
{
Debug.LogWarning("socket err " + e.ErrorCode);
}
if (recvBytes > 0)
{
MAVLink.MAVLinkMessage msg = mavlinkParse.ReadPacket(buf);
if (msg != null)
{
if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.STATUSTEXT)
{
var status_txt = (MAVLink.mavlink_statustext_t)msg.data;
Debug.Log(System.Text.Encoding.ASCII.GetString(status_txt.text));
}
else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
var heartbeat = (MAVLink.mavlink_heartbeat_t)msg.data;
apm_mode = (int)heartbeat.custom_mode;
if (apm_mode != (int)MAVLink.COPTER_MODE.GUIDED)
{
wp.SetActive(false);
}
if ((heartbeat.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == 0)
{
if (armed)
{
MyDroneModel.GetComponent <DroneAnime>().PropellerRun = false;
}
armed = false;
}
else
{
if (!armed)
{
MyDroneModel.GetComponent <DroneAnime>().PropellerRun = true;
}
armed = true;
}
system_status = heartbeat.system_status;
if (!pos_tgt_local_rcved && (apm_mode == (int)MAVLink.COPTER_MODE.GUIDED))
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t
{
command = (ushort)MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL,
param1 = (float)MAVLink.MAVLINK_MSG_ID.POSITION_TARGET_LOCAL_NED,
param2 = 1000000
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
sock.SendTo(data, myproxy);
}
if (!mis_cur_rcved && (apm_mode == (int)MAVLink.COPTER_MODE.AUTO))
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t
{
command = (ushort)MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL,
param1 = (float)MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT,
param2 = 1000000
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
sock.SendTo(data, myproxy);
}
if (mission_count < 0)
{
MAVLink.mavlink_mission_request_list_t cmd = new MAVLink.mavlink_mission_request_list_t
{
target_system = 0,
target_component = 0,
mission_type = 0
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_LIST, cmd);
sock.SendTo(data, myproxy);
Debug.Log("send MISSION_REQUEST_LIST");
}
if ((wait_mission_seq >= 0) && (wait_mission_seq < mission_count))
{
MAVLink.mavlink_mission_request_int_t cmd = new MAVLink.mavlink_mission_request_int_t
{
target_system = 0,
target_component = 0,
seq = (ushort)wait_mission_seq,
mission_type = 0
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_INT, cmd);
sock.SendTo(data, myproxy);
Debug.Log("send MISSION_REQUEST_INT " + wait_mission_seq);
}
}
else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.POSITION_TARGET_LOCAL_NED)
{
pos_tgt_local_rcved = true;
var pos_tgt = (MAVLink.mavlink_position_target_local_ned_t)msg.data;
if (((pos_tgt.type_mask & 0x1000) == 0x1000) || system_status != (byte)MAVLink.MAV_STATE.ACTIVE)
{
wp.SetActive(false);
}
else
{
wp.transform.position = new Vector3(-pos_tgt.x, -pos_tgt.z, pos_tgt.y);
wp.SetActive(true);
}
}
else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT)
{
mis_cur_rcved = true;
cur_mis_seq = ((MAVLink.mavlink_mission_current_t)msg.data).seq;
//Debug.Log("rcv MISSION_CURRENT " + cur_mis_seq);
if (cur_mis_seq < nxt_wp_seq)
{
MAVLink.mavlink_mission_set_current_t cmd = new MAVLink.mavlink_mission_set_current_t
{
target_system = 0,
target_component = 0,
seq = (ushort)nxt_wp_seq
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_SET_CURRENT, cmd);
sock.SendTo(data, myproxy);
}
else if (mission_chk_points.Contains(cur_mis_seq))
{
waiting_in_chk_point = true;
}
else
{
waiting_in_chk_point = false;
}
}
else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_COUNT)
{
mission_count = ((MAVLink.mavlink_mission_count_t)msg.data).count;
if (mission_count > 0)
{
MAVLink.mavlink_mission_request_int_t cmd = new MAVLink.mavlink_mission_request_int_t
{
target_system = 0,
target_component = 0,
seq = 0,
mission_type = 0
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_INT, cmd);
sock.SendTo(data, myproxy);
wait_mission_seq = 0;
Debug.Log("send MISSION_REQUEST_INT 0");
}
}
else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_INT)
{
var item_int = (MAVLink.mavlink_mission_item_int_t)msg.data;
if (item_int.seq == wait_mission_seq)
{
if (item_int.command == 93) //MAV_CMD_NAV_DELAY
{
mission_chk_points.Add(item_int.seq);
}
wait_mission_seq += 1;
if (wait_mission_seq < mission_count)
{
MAVLink.mavlink_mission_request_int_t cmd = new MAVLink.mavlink_mission_request_int_t
{
target_system = 0,
target_component = 0,
seq = (ushort)wait_mission_seq,
mission_type = 0
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_INT, cmd);
sock.SendTo(data, myproxy);
Debug.Log("send MISSION_REQUEST_INT " + wait_mission_seq);
}
else
{
MAVLink.mavlink_mission_ack_t cmd = new MAVLink.mavlink_mission_ack_t
{
target_system = 0,
target_component = 0,
type = 0,
mission_type = 0
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_ACK, cmd);
sock.SendTo(data, myproxy);
Debug.Log("send MISSION_ACK");
}
}
}
/*else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_REACHED)
* {
* Debug.Log("rcv MISSION_ITEM_REACHED "+((MAVLink.mavlink_mission_item_reached_t)msg.data).seq);
* int seq = ((MAVLink.mavlink_mission_item_reached_t)msg.data).seq;
* if (mis_chk_points.Contains(seq))
* {
* MAVLink.mavlink_mission_set_current_t cmd = new MAVLink.mavlink_mission_set_current_t
* {
* target_system = 0,
* target_component = 0,
* seq = (ushort)(seq + 2)
* };
* byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_SET_CURRENT, cmd);
* sock.SendTo(data, myproxy);
* }
* }*/
}
}
}
if (lastPos.Equals(transform.localPosition))
{
tracking = false;
}
else
{
tracking = true;
vel_update_cd -= Time.deltaTime;
if (vel_update_cd < 0)
{
vel_update_cd = 0.5f;
vel = (transform.localPosition - lastPos) / Time.deltaTime;
}
}
lastPos = transform.localPosition;
if (!tracking)
{
if (MyDroneModel.activeSelf)
{
MyDroneModel.SetActive(false);
GetComponent <BoxCollider>().enabled = false;
}
}
else if (!MyDroneModel.activeSelf)
{
MyDroneModel.SetActive(true);
GetComponent <BoxCollider>().enabled = true;
}
}
private void ListenForData()
{
try {
socketConnection = new TcpClient("localhost", 14550);
Byte[] bytes = new Byte[1024];
Debug.Log("LISTENING!!!!");
using (NetworkStream stream = socketConnection.GetStream()) {
while (true)
{
// Get a stream object for reading
//int length;
MAVLink.MavlinkParse parser = new MAVLink.MavlinkParse();
MAVLink.MAVLinkMessage message = parser.ReadPacket(stream);
/*// Read incomming stream into byte arrary.
* if (message.data is MAVLink.mavlink_vfr_hud_t hud) {
* data["heading"] = hud.heading.ToString();
* data["airspeed"] = hud.airspeed.ToString();
* data["altitude"] = hud.alt;
*
* }
* if (message.data is MAVLink.mavlink_high_latency_t newData) {
* data["longitude"] = newData.longitude.ToString();
* }
* } */
switch (message.data)
{
case MAVLink.mavlink_vfr_hud_t newData:
data["heading"] = newData.heading.ToString();
data["airspeed"] = this.FormatNumber(newData.airspeed.ToString());
break;
case MAVLink.mavlink_global_position_int_t newData:
var longitudeString = newData.lon.ToString();
data["longitude"] = longitudeString.Substring(0, longitudeString.Length - 7) + "." + longitudeString.Substring(longitudeString.Length - 7);
var latitudeString = newData.lat.ToString();
data["latitude"] = latitudeString.Substring(0, latitudeString.Length - 7) + "." + latitudeString.Substring(latitudeString.Length - 7);
var altitudeString = newData.alt.ToString();
data["altitude"] = this.FormatNumber(altitudeString.Substring(0, altitudeString.Length - 4) + "." + altitudeString.Substring(altitudeString.Length - 4));
//var relString = newData.relative_alt.ToString();
//var rel = this.FormatNumber(relString.Substring(0, relString.Length - 4) + "." + relString.Substring(relString.Length - 4));
if (!initialPositionRecieved && get("longitude") != "" && get("latitude") != "" && get("altitude") != "")
{
initialPositionRecieved = true;
}
break;
case MAVLink.mavlink_attitude_t newData:
data["roll"] = this.FormatNumber(newData.roll.ToString());
data["pitch"] = this.FormatNumber(newData.pitch.ToString());
data["yaw"] = this.FormatNumber(newData.yaw.ToString());
break;
}
//Debug.Log(message.msgtypename);
}
}
}
catch (SocketException socketException) {
Debug.Log("Socket exception: " + socketException);
}
}
public static void SortLogs(string[] logs, string masterdestdir = "")
{
Parallel.ForEach(logs, logfile =>
//foreach (var logfile in logs)
{
if (masterdestdir == "")
{
masterdestdir = Path.GetDirectoryName(logfile);
}
try
{
FileInfo info;
info = new FileInfo(logfile);
// delete 0 size files
if (info.Length == 0)
{
try
{
File.Delete(logfile);
}
catch
{
}
return;
}
// move small logs - most likerly invalid
if (info.Length <= 1024)
{
try
{
string destdir = masterdestdir + Path.DirectorySeparatorChar
+ "SMALL" + Path.DirectorySeparatorChar;
if (!Directory.Exists(destdir))
{
Directory.CreateDirectory(destdir);
}
log.Info("Move log small " + logfile + " to " + destdir + Path.GetFileName(logfile));
MoveFileUsingMask(logfile, destdir);
}
catch
{
}
return;
}
bool issitl = false;
var sysid = 0;
var compid = 0;
var aptype = MAVLink.MAV_TYPE.GENERIC;
var packetsseen = 0;
var parse = new MAVLink.MavlinkParse(true);
using (var binfile = File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read))
{
var midpoint = binfile.Length / 8;
binfile.Seek(midpoint, SeekOrigin.Begin);
MAVLink.MAVLinkMessage packet;
List <MAVLink.MAVLinkMessage> hblist = new List <MAVLink.MAVLinkMessage>();
try
{
var length = binfile.Length;
while (binfile.Position < length)
{
packet = parse.ReadPacket(binfile);
// no gcs packets
if (packet == null || packet.compid == 190)
{
continue;
}
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.SIMSTATE)
{
packetsseen++;
issitl = true;
}
else if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
packetsseen++;
hblist.Add(packet);
var hb = (MAVLink.mavlink_heartbeat_t)packet.data;
sysid = packet.sysid;
compid = packet.compid;
aptype = (MAVLink.MAV_TYPE)hb.type;
if (hblist.Count > 10)
{
break;
}
}
else if (packet != MAVLink.MAVLinkMessage.Invalid)
{
packetsseen++;
if (sysid == 0)
{
sysid = packet.sysid;
}
if (compid == 0)
{
compid = packet.compid;
}
}
}
} catch (EndOfStreamException) { }
catch { }
if (hblist.Count == 0)
{
binfile.Close();
if (!Directory.Exists(masterdestdir + Path.DirectorySeparatorChar + "BAD"))
{
Directory.CreateDirectory(masterdestdir + Path.DirectorySeparatorChar +
"BAD");
}
log.Info("Move log bad " + logfile + " to " + masterdestdir +
Path.DirectorySeparatorChar + "BAD" + Path.DirectorySeparatorChar +
Path.GetFileName(logfile) + " " + info.Length);
MoveFileUsingMask(logfile,
masterdestdir + Path.DirectorySeparatorChar + "BAD" +
Path.DirectorySeparatorChar);
return;
}
// find most appropriate
if (hblist.GroupBy(a => a.sysid * 256 + a.compid).ToArray().Length > 1)
{
foreach (var mav in hblist)
{
var hb = (MAVLink.mavlink_heartbeat_t)mav.data;
if (hb.type == (byte)MAVLink.MAV_TYPE.ANTENNA_TRACKER)
{
continue;
}
if (hb.type == (byte)MAVLink.MAV_TYPE.GCS)
{
continue;
}
sysid = mav.sysid;
compid = mav.compid;
aptype = (MAVLink.MAV_TYPE)hb.type;
}
}
binfile.Close();
string destdir = masterdestdir + Path.DirectorySeparatorChar
+ aptype.ToString() + Path.DirectorySeparatorChar
+ sysid + Path.DirectorySeparatorChar;
if (issitl)
{
destdir = masterdestdir + Path.DirectorySeparatorChar
+ "SITL" + Path.DirectorySeparatorChar
+ aptype.ToString() + Path.DirectorySeparatorChar
+ sysid + Path.DirectorySeparatorChar;
}
if (!Directory.Exists(destdir))
{
Directory.CreateDirectory(destdir);
}
MoveFileUsingMask(logfile, destdir);
}
}
catch
{
return;
}
});
}