public void Add(MAVLink.MAVLinkMessage message)
{
var id = GetID(message.sysid, message.compid);
lock (_lock)
{
if (!_history.ContainsKey(id))
{
Clear(message.sysid, message.compid);
}
_history[id][message.msgid] = message;
if (!_rate[id].ContainsKey(message.msgid))
{
_rate[id][message.msgid] = new List <irate>();
}
_rate[id][message.msgid].Add(new irate(DateTime.Now, 1));
while (_rate[id][message.msgid].Count > RateHistory)
{
_rate[id][message.msgid].RemoveAt(0);
}
}
}
private bool ProcessCompassMotMSG(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.COMPASSMOT_STATUS)
{
var status = (MAVLink.mavlink_compassmot_status_t)arg.data;
interferencelist.Add(status.throttle / 10.0, status.interference);
currentlist.Add(status.throttle / 10.0, status.current);
interferencelist.Sort();
currentlist.Sort();
var msg = "Current: " + status.current.ToString("0.00") + "\nx,y,z " +
status.CompensationX.ToString("0.00") + "," +
status.CompensationY.ToString("0.00") +
"," + status.CompensationZ.ToString("0.00") + "\nThrottle: " +
(status.throttle / 10.0) +
"\nInterference: " + status.interference;
this.BeginInvokeIfRequired(() =>
{
lbl_status.Text = msg;
interference.Points = interferencelist;
current.Points = currentlist;
zedGraphControl1.AxisChange();
zedGraphControl1.Invalidate();
});
}
return(true);
}
bool ReceviedPacket(MAVLink.MAVLinkMessage rawpacket)
{
if (rawpacket.msgid == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST)
{
MAVLink.mavlink_terrain_request_t packet =
rawpacket.ByteArrayToStructure <MAVLink.mavlink_terrain_request_t>();
if (issending)
{
return(false);
}
lastrequest = packet;
log.Info("received TERRAIN_REQUEST " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " space " +
packet.grid_spacing + " " + Convert.ToString((long)packet.mask, 2));
System.Threading.ThreadPool.QueueUserWorkItem(QueueSendGrid);
}
else if (rawpacket.msgid == (byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT)
{
MAVLink.mavlink_terrain_report_t packet =
rawpacket.ByteArrayToStructure <MAVLink.mavlink_terrain_report_t>();
log.Info("received TERRAIN_REPORT " + packet.lat / 1e7 + " " + packet.lon / 1e7 + " " + packet.loaded + " " +
packet.pending);
}
return(false);
}
private bool PacketResponse(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.OSD_PARAM_CONFIG_REPLY)
{
var rep = (MAVLink.mavlink_osd_param_config_reply_t)arg.data;
if (rep.result != (byte)MAVLink.OSD_PARAM_CONFIG_ERROR.OSD_PARAM_SUCCESS)
{
CustomMessageBox.Show("OSD Config set Error", Strings.ERROR);
}
}
else if (arg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.OSD_PARAM_SHOW_CONFIG_REPLY)
{
var rep = (MAVLink.mavlink_osd_param_show_config_reply_t)arg.data;
if (rep.result != (byte)MAVLink.OSD_PARAM_CONFIG_ERROR.OSD_PARAM_SUCCESS)
{
CustomMessageBox.Show("OSD Config show Error", Strings.ERROR);
}
else
{
var param = (rep.param_id, rep.config_type, rep.min_value, rep.max_value, rep.increment);
@params.Add(param);
}
}
return(true);
}
// 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 = heartbeat.custom_mode;
armed = (heartbeat.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) != 0;
}
}
}
}
}
private void but_gpsinj_Click(object sender, EventArgs e)
{
OpenFileDialog ofd = new OpenFileDialog();
ofd.Filter = "tlog|*.tlog";
ofd.ShowDialog();
SaveFileDialog sfd = new SaveFileDialog();
sfd.FileName = "output.dat";
sfd.ShowDialog();
if (ofd.CheckFileExists)
{
using (var st = sfd.OpenFile())
{
using (MAVLinkInterface mine = new MAVLinkInterface(ofd.OpenFile()))
{
mine.logreadmode = true;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
MAVLink.MAVLinkMessage packet = mine.readPacket();
if (packet.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GPS_INJECT_DATA)
{
var item = packet.ToStructure <MAVLink.mavlink_gps_inject_data_t>();
st.Write(item.data, 0, item.len);
}
}
}
}
}
}
private bool ReceviedPacket(MAVLink.MAVLinkMessage packet)
{
if (System.Diagnostics.Debugger.IsAttached)
{
MainV2.comPort.DebugPacket(packet, true);
}
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MAG_CAL_PROGRESS)
{
lock (this.mprog)
{
this.mprog.Add(packet);
}
return(true);
}
else if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MAG_CAL_REPORT)
{
lock (this.mrep)
{
this.mrep.Add(packet);
}
return(true);
}
return(true);
}
bool ReceviedPacket(MAVLink.MAVLinkMessage packet)
{
if (packetcounttimer.Second != DateTime.Now.Second)
{
log.Info("packet count " + packetcount + " with data " + packetwithdata + " " + buffer.Size);
packetcount = 0;
packetwithdata = 0;
packetcounttimer = DateTime.Now;
}
packetcount++;
MAVLink.mavlink_serial_control_t item = packet.ToStructure <MAVLink.mavlink_serial_control_t>();
if (item.count == 0)
{
return(true);
}
packetwithdata++;
Console.WriteLine(DateTime.Now.Millisecond + "data count " + item.count);
// ASCIIEncoding.ASCII.GetString(item.data, 0, item.count)
lock (buffer)
{
buffer.AllowOverflow = true;
buffer.Put(item.data, 0, item.count);
}
// we just received data, so do a request again
GetData(true);
return(true);
}
private bool ReceviedPacket(MAVLink.MAVLinkMessage packet)
{
if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MAG_CAL_PROGRESS)
{
var mprog = packet.ToStructure <MAVLink.mavlink_mag_cal_progress_t>();
lock (this.mprog)
{
this.mprog.Add(mprog);
}
return(true);
}
else if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MAG_CAL_REPORT)
{
var mrep = packet.ToStructure <MAVLink.mavlink_mag_cal_report_t>();
lock (this.mrep)
{
this.mrep.Add(mrep);
}
return(true);
}
return(true);
}
bool ReceviedPacket(MAVLink.MAVLinkMessage rawpacket)
{
if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST)
{
MAVLink.mavlink_terrain_request_t packet =
rawpacket.ToStructure<MAVLink.mavlink_terrain_request_t>();
if (issending)
return false;
lastmessage = rawpacket;
lastrequest = packet;
log.Info("received TERRAIN_REQUEST " + packet.lat/1e7 + " " + packet.lon/1e7 + " space " +
packet.grid_spacing + " " + Convert.ToString((long) packet.mask, 2));
System.Threading.ThreadPool.QueueUserWorkItem(QueueSendGrid);
}
else if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT)
{
MAVLink.mavlink_terrain_report_t packet =
rawpacket.ToStructure<MAVLink.mavlink_terrain_report_t>();
log.Info("received TERRAIN_REPORT " + packet.lat/1e7 + " " + packet.lon/1e7 + " " + packet.loaded + " " +
packet.pending);
}
return false;
}
private bool receivedPacket(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.STATUSTEXT)
{
var message = ASCIIEncoding.ASCII.GetString(arg.ToStructure <MAVLink.mavlink_statustext_t>().text);
UpdateUserMessage(message);
if (message.ToLower().Contains("calibration successful") ||
message.ToLower().Contains("calibration failed"))
{
try
{
Invoke((MethodInvoker) delegate
{
BUT_calib_accell.Text = Strings.Done;
BUT_calib_accell.Enabled = false;
});
_incalibrate = false;
MainV2.comPort.UnSubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.STATUSTEXT, receivedPacket);
}
catch
{
}
}
}
return(true);
}
// 该方法在GCS开启的子线程中执行
private void MessageHandler(MAVLink.MAVLinkMessage packet)
{
// Console.WriteLine(packet.msgtypename + " ##" + packet.msgid);
// BeginInvoke(updator, packet); 若要实现C#的GUI版本，取消该行注释
server.SendMessage(JsonConvert.SerializeObject(packet));
}
private void _interface_OnPacketReceived(object sender, MAVLink.MAVLinkMessage e)
{
if (e.msgid == (uint)MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST ||
e.msgid == (uint)MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT)
{
ReceivedPacket(e);
}
}
private void MavOnOnPacketReceivedHandler(object o, MAVLink.MAVLinkMessage linkMessage)
{
if ((MAVLink.MAVLINK_MSG_ID)linkMessage.msgid == MAVLink.MAVLINK_MSG_ID.BUTTON_CHANGE)
{
//Button change message received
//Check and display message
Console.WriteLine("BUTTON CHANGE message received");
}
}
private void MavOnOnPacketReceivedHandler(object o, MAVLink.MAVLinkMessage linkMessage)
{
//Pull force at the drone is coming down as a Mavlink DEBUG message
if ((MAVLink.MAVLINK_MSG_ID)linkMessage.msgid == MAVLink.MAVLINK_MSG_ID.DEBUG)
{
var status = linkMessage.ToStructure <MAVLink.mavlink_debug_t>();
tension_value = status.value;
}
}
private bool ReceviedPacket(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (byte)MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE)
{
var packet = arg.ToStructure <MAVLink.mavlink_data_transmission_handshake_t>();
msgDataTransmissionHandshake = packet;
imageBuffer.Close();
imageBuffer = new MemoryStream((int)packet.size);
}
else if (arg.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA)
{
var packet = arg.ToStructure <MAVLink.mavlink_encapsulated_data_t>();
msgEncapsulatedData = packet;
int start = msgDataTransmissionHandshake.payload * msgEncapsulatedData.seqnr;
int left = (int)msgDataTransmissionHandshake.size - start;
var writeamount = Math.Min(msgEncapsulatedData.data.Length, left);
imageBuffer.Seek(start, SeekOrigin.Begin);
imageBuffer.Write(msgEncapsulatedData.data, 0, writeamount);
// we have a complete image
if ((msgEncapsulatedData.seqnr + 1) == msgDataTransmissionHandshake.packets)
{
using (
Bitmap bmp = new Bitmap(msgDataTransmissionHandshake.width, msgDataTransmissionHandshake.height,
PixelFormat.Format8bppIndexed))
{
SetGrayscalePalette(bmp);
var bitmapData = bmp.LockBits(new Rectangle(Point.Empty, bmp.Size), ImageLockMode.ReadWrite,
bmp.PixelFormat);
if (imageBuffer.Length > msgDataTransmissionHandshake.size)
{
return(true);
}
var buffer = imageBuffer.ToArray();
Marshal.Copy(buffer, 0, bitmapData.Scan0, buffer.Length);
bmp.UnlockBits(bitmapData);
if (newImage != null)
{
newImage(this, new ImageEventHandle(bmp));
}
//bmp.Save("test.bmp", ImageFormat.Bmp);
}
}
}
return(true);
}
private bool ReceviedPacket(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (byte)MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE)
{
var packet = arg.ToStructure <MAVLink.mavlink_data_transmission_handshake_t>();
msgDataTransmissionHandshake = packet;
imageBuffer.Close();
imageBuffer = new MemoryStream((int)packet.size);
}
else if (arg.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA)
{
var packet = arg.ToStructure <MAVLink.mavlink_encapsulated_data_t>();
msgEncapsulatedData = packet;
int start = msgDataTransmissionHandshake.payload * msgEncapsulatedData.seqnr;
int left = (int)msgDataTransmissionHandshake.size - start;
var writeamount = Math.Min(msgEncapsulatedData.data.Length, left);
imageBuffer.Seek(start, SeekOrigin.Begin);
imageBuffer.Write(msgEncapsulatedData.data, 0, writeamount);
// we have a complete image
if ((msgEncapsulatedData.seqnr + 1) == msgDataTransmissionHandshake.packets)
{
using (
Bitmap bmp = new Bitmap(msgDataTransmissionHandshake.width, msgDataTransmissionHandshake.height))
{
SetGrayscalePalette(bmp);
if (imageBuffer.Length > msgDataTransmissionHandshake.size)
{
return(true);
}
var buffer = imageBuffer.ToArray();
int a = 0;
foreach (var b in buffer)
{
bmp.SetPixel(a % bmp.Width, (int)(a / bmp.Width), bmp.Palette.Entries[b]);
a++;
}
if (newImage != null)
{
newImage(this, new ImageEventHandle(bmp));
}
//bmp.Save("test.bmp", ImageFormat.Bmp);
}
}
}
return(true);
}
public void Add(MAVLink.MAVLinkMessage message)
{
var id = GetID(message.sysid, message.compid);
if (!_history.ContainsKey(id))
{
Clear(message.sysid, message.compid);
}
_history[id][message.msgid] = message;
Console.WriteLine(message.ToString());
}
private bool receivedPacket(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.STATUSTEXT)
{
var message = ASCIIEncoding.ASCII.GetString(arg.ToStructure <MAVLink.mavlink_statustext_t>().text);
UpdateUserMessage(message);
if (message.ToLower().Contains("calibration successful") ||
message.ToLower().Contains("calibration failed"))
{
try
{
Invoke((MethodInvoker) delegate
{
imageLabel1.Text = Strings.Done;
BUT_continue.Enabled = false;
BUT_start.Enabled = true;
});
busy = false;
MainV2.comPort.UnSubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.STATUSTEXT, receivedPacket);
MainV2.comPort.UnSubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, receivedPacket);
}
catch
{
}
}
}
if (arg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG)
{
var message = arg.ToStructure <MAVLink.mavlink_command_long_t>();
if (message.command == (ushort)MAVLink.MAV_CMD.ACCELCAL_VEHICLE_POS)
{
MAVLink.ACCELCAL_VEHICLE_POS pos = (MAVLink.ACCELCAL_VEHICLE_POS)message.param1;
UpdateUserMessage("Please place vehicle " + pos.ToString());
}
}
return(true);
}
private void Port_OnPacketReceived(object sender, MAVLink.MAVLinkMessage message)
{
if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.REMOTE_LOG_DATA_BLOCK)
{
var data = (MAVLink.mavlink_remote_log_data_block_t)message.data;
seq = data.seqno;
if (logfilestream.Position != data.seqno * 200)
{
logfilestream.Seek(data.seqno * 200, SeekOrigin.Begin);
}
logfilestream.Write(data.data, 0, data.data.Length);
var resp = new MAVLink.mavlink_remote_log_block_status_t(data.seqno, message.sysid, message.compid,
(byte)MAVLink.MAV_REMOTE_LOG_DATA_BLOCK_STATUSES.MAV_REMOTE_LOG_DATA_BLOCK_ACK);
port.sendPacket(resp, message.sysid, message.compid);
}
}
private void MavOnOnPacketReceived(object o, MAVLink.MAVLinkMessage linkMessage)
{
mavi.Add(linkMessage.sysid, linkMessage.compid, linkMessage.msgid, linkMessage, linkMessage.Length);
}
/// <summary>
/// Event handler for retreiving gain in each
/// direction and for retreiving current waypoints
/// </summary>
/// <param name="data">A MAVLink message of the data.</param>
/// <returns></returns>
private static void MavLinkPacketReceived_Handler(object o, MAVLink.MAVLinkMessage msg)
{
if (msg.sysid == system_id && msg.compid == comp_id) //Get Pi messages
{
if (msg.msgid == (int)MAVLink.MAVLINK_MSG_ID.COMMAND_ACK)
{
MAVLink.mavlink_command_ack_t cmd_ack_msg = (MAVLink.mavlink_command_ack_t)msg.data;
scan_completion_status = cmd_ack_msg.result;
}
if (msg.msgid == (int)MAVLink.MAVLINK_MSG_ID.PARAM_VALUE)
{
MAVLink.mavlink_param_value_t param_value_msg = (MAVLink.mavlink_param_value_t)msg.data;
string param_id = System.Text.Encoding.Default.GetString(param_value_msg.param_id).Trim().ToUpper();
if (param_id[0] == 'V' && param_id[1] == 'H' &&
param_id[2] == 'F' && param_id[3] == '_' &&
param_id[4] == 'S' && param_id[5] == 'N' &&
param_id[6] == 'R')
{
float SNR = param_value_msg.param_value;
int direction = (int)(Math.Round(MavLinkCom.MAV.cs.yaw / 5.0) * 5);
RDFData.Add(new KeyValuePair <int, float>(direction, SNR));
RDFDataReceived(new object(), new EventArgs());
//Set to unsupported so that a retrigger of a new packet
//does not happen.
scan_completion_status = (byte)MAVLink.MAV_RESULT.UNSUPPORTED;
vhf_snr_state_changed = true;
}
else if (param_id[0] == 'V' && param_id[1] == 'H' &&
param_id[2] == 'F' && param_id[3] == '_' &&
param_id[4] == 'F' && param_id[5] == 'R' &&
param_id[6] == 'E' && param_id[7] == 'Q')
{
vhf_freq_state_changed = true;
}
else if (param_id[0] == 'I' && param_id[1] == 'F')
{
if_gain_state_changed = true;
}
else if (param_id[0] == 'M' && param_id[1] == 'I' &&
param_id[2] == 'X')
{
mixer_gain_state_changed = true;
}
else if (param_id[0] == 'L' && param_id[1] == 'N' &&
param_id[2] == 'A')
{
lna_gain_state_changed = true;
}
else if (param_id[0] == 'V' && param_id[1] == 'H' &&
param_id[2] == 'F' && param_id[3] == '_' &&
param_id[4] == 'R' && param_id[5] == 'S' &&
param_id[6] == 'T' && param_id[7] == 'T')
{
SendMavLinkFrequency(vhf_freq);
SendMavLinkIFGain(if_gain);
SendMavLinkMixerGain(mixer_gain);
SendMavLinkLNAGain(lna_gain);
}
}
}
}
public static void MapLogs(string[] logs)
{
foreach (var logfile in logs)
{
if (File.Exists(logfile + ".jpg"))
{
continue;
}
double minx = 99999;
double maxx = -99999;
double miny = 99999;
double maxy = -99999;
Dictionary <int, List <PointLatLngAlt> > loc_list = new Dictionary <int, List <PointLatLngAlt> >();
try
{
if (logfile.ToLower().EndsWith(".tlog"))
{
using (MAVLinkInterface mine = new MAVLinkInterface())
using (
mine.logplaybackfile =
new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite))
)
{
mine.logreadmode = true;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
MAVLink.MAVLinkMessage packet = mine.readPacket();
if (packet.Length < 5)
{
continue;
}
try
{
if (MainV2.speechEngine != null)
{
MainV2.speechEngine.SpeakAsyncCancelAll();
}
}
catch
{
}
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 = MAVList.GetID(packet.sysid, 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);
}
}
}
}
else if (logfile.ToLower().EndsWith(".bin") || logfile.ToLower().EndsWith(".log"))
{
bool bin = logfile.ToLower().EndsWith(".bin");
BinaryLog binlog = new BinaryLog();
DFLog dflog = new DFLog();
using (var st = File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.ReadWrite))
{
using (StreamReader sr = new StreamReader(st))
{
loc_list[0] = new List <PointLatLngAlt>();
while (sr.BaseStream.Position < sr.BaseStream.Length)
{
string line = "";
if (bin)
{
line = binlog.ReadMessage(sr.BaseStream);
}
else
{
line = sr.ReadLine();
}
if (line.StartsWith("FMT"))
{
dflog.FMTLine(line);
}
else if (line.StartsWith("GPS"))
{
var item = dflog.GetDFItemFromLine(line, 0);
var lat = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lat")]);
var lon = double.Parse(item.items[dflog.FindMessageOffset(item.msgtype, "Lng")]);
if (lat == 0 || lon == 0)
{
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.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);
var map = GetMap(area);
var grap = Graphics.FromImage(map);
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", System.Drawing.Imaging.ImageFormat.Jpeg);
map.Dispose();
map = null;
}
else
{
DoTextMap(logfile + ".jpg", "No gps data");
}
}
catch (Exception ex)
{
if (ex.ToString().Contains("Mavlink 0.9"))
{
DoTextMap(logfile + ".jpg", "Old log\nMavlink 0.9");
}
continue;
}
}
}
static string process(MAVLinkInterface mavint)
{
DateTime Deadline = DateTime.Now.AddSeconds(60);
Vector3 last_mag = null;
double last_usec = 0;
double count = 0;
double[] total_error = new double[rotations.Length];
float AHRS_ORIENTATION = 0;
float COMPASS_ORIENT = 0;
float COMPASS_EXTERNAL = 0;
//# now gather all the data
while (DateTime.Now < Deadline || mavint.BaseStream.BytesToRead > 0)
{
MAVLink.MAVLinkMessage packetbytes = mavint.readPacket();
if (packetbytes.Length < 5)
{
continue;
}
object packet = packetbytes.data;
if (packet == null)
{
continue;
}
if (packet is MAVLink.mavlink_param_value_t)
{
MAVLink.mavlink_param_value_t m = (MAVLink.mavlink_param_value_t)packet;
if (str(m.param_id) == "AHRS_ORIENTATION")
{
AHRS_ORIENTATION = (int)(m.param_value);
}
if (str(m.param_id) == "COMPASS_ORIENT")
{
COMPASS_ORIENT = (int)(m.param_value);
}
if (str(m.param_id) == "COMPASS_EXTERNAL")
{
COMPASS_EXTERNAL = (int)(m.param_value);
}
}
if (packet is MAVLink.mavlink_raw_imu_t)
{
MAVLink.mavlink_raw_imu_t m = (MAVLink.mavlink_raw_imu_t)packet;
Vector3 mag = new Vector3(m.xmag, m.ymag, m.zmag);
mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL);
Vector3 gyr = new Vector3(m.xgyro, m.ygyro, m.zgyro) * 0.001;
double usec = m.time_usec;
if (last_mag != null && gyr.length() > radians(5.0))
{
add_errors(mag, gyr, last_mag, (usec - last_usec) * 1.0e-6, total_error, rotations);
count += 1;
}
last_mag = mag;
last_usec = usec;
}
}
int best_i = 0;
double best_err = total_error[0];
foreach (var i in range(len(rotations)))
{
Rotation r = rotations[i];
// if (!r.is_90_degrees())
// continue;
//if ( opts.verbose) {
// print("%s err=%.2f" % (r, total_error[i]/count));}
if (total_error[i] < best_err)
{
best_i = i;
best_err = total_error[i];
}
}
Rotation rans = rotations[best_i];
Console.WriteLine("Best rotation is {0} err={1} from {2} points", rans, best_err / count, count);
//print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count));
return(rans.name);
}
string GetLog(ushort no, string fileName)
{
log.Info("GetLog " + no);
MainV2.comPort.Progress += comPort_Progress;
status = SerialStatus.Reading;
// used for log fn
MAVLink.MAVLinkMessage hbpacket = MainV2.comPort.getHeartBeat();
if (hbpacket != null)
{
log.Info("Got hbpacket length: " + hbpacket.Length);
}
// get df log from mav
using (var ms = MainV2.comPort.GetLog(no))
{
if (ms != null)
{
log.Info("Got Log length: " + ms.Length);
}
ms.Seek(0, SeekOrigin.Begin);
status = SerialStatus.Done;
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)MainV2.comPort.DebugPacket(hbpacket);
logfile = Settings.Instance.LogDir + Path.DirectorySeparatorChar
+ MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar
+ hbpacket.sysid + Path.DirectorySeparatorChar + no + " " + MakeValidFileName(fileName) + ".bin";
// make log dir
Directory.CreateDirectory(Path.GetDirectoryName(logfile));
log.Info("about to write: " + logfile);
// save memorystream to file
using (BinaryWriter bw = new BinaryWriter(File.OpenWrite(logfile)))
{
byte[] buffer = new byte[256 * 1024];
while (ms.Position < ms.Length)
{
int read = ms.Read(buffer, 0, buffer.Length);
bw.Write(buffer, 0, read);
}
}
}
log.Info("about to convertbin: " + logfile);
// create ascii log
BinaryLog.ConvertBin(logfile, logfile + ".log");
//update the new filename
logfile = logfile + ".log";
// rename file if needed
log.Info("about to GetFirstGpsTime: " + logfile);
// get gps time of assci log
DateTime logtime = new DFLog().GetFirstGpsTime(logfile);
// rename log is we have a valid gps time
if (logtime != DateTime.MinValue)
{
string newlogfilename = Settings.Instance.LogDir + Path.DirectorySeparatorChar
+ MainV2.comPort.MAV.aptype.ToString() + Path.DirectorySeparatorChar
+ hbpacket.sysid + Path.DirectorySeparatorChar +
logtime.ToString("yyyy-MM-dd HH-mm-ss") + ".log";
try
{
File.Move(logfile, newlogfilename);
// rename bin as well
File.Move(logfile.Replace(".log", ""), newlogfilename.Replace(".log", ".bin"));
logfile = newlogfilename;
}
catch
{
CustomMessageBox.Show(Strings.ErrorRenameFile + " " + logfile + "\nto " + newlogfilename,
Strings.ERROR);
}
}
MainV2.comPort.Progress -= comPort_Progress;
return(logfile);
}
public static void Main(string[] args)
{
Program.args = args;
Console.WriteLine(
"If your error is about Microsoft.DirectX.DirectInput, please install the latest directx redist from here http://www.microsoft.com/en-us/download/details.aspx?id=35 \n\n");
Console.WriteLine("Debug under mono MONO_LOG_LEVEL=debug mono MissionPlanner.exe");
var t = Type.GetType("Mono.Runtime");
MONO = (t != null);
Thread = Thread.CurrentThread;
System.Windows.Forms.Application.EnableVisualStyles();
XmlConfigurator.Configure();
log.Info("******************* Logging Configured *******************");
System.Windows.Forms.Application.SetCompatibleTextRenderingDefault(false);
ServicePointManager.DefaultConnectionLimit = 10;
System.Windows.Forms.Application.ThreadException += Application_ThreadException;
// fix ssl on mono
ServicePointManager.ServerCertificateValidationCallback =
new System.Net.Security.RemoteCertificateValidationCallback(
(sender, certificate, chain, policyErrors) => { return(true); });
if (args.Length > 0 && args[0] == "/update")
{
Utilities.Update.DoUpdate();
return;
}
name = "Mission Planner";
try
{
if (File.Exists(Settings.GetRunningDirectory() + "logo.txt"))
{
name = File.ReadAllLines(Settings.GetRunningDirectory() + "logo.txt",
Encoding.UTF8)[0];
}
}
catch
{
}
if (File.Exists(Settings.GetRunningDirectory() + "logo.png"))
{
Logo = new Bitmap(Settings.GetRunningDirectory() + "logo.png");
}
if (File.Exists(Settings.GetRunningDirectory() + "logo2.png"))
{
Logo2 = new Bitmap(Settings.GetRunningDirectory() + "logo2.png");
}
if (File.Exists(Settings.GetRunningDirectory() + "icon.png"))
{
// 128*128
IconFile = new Bitmap(Settings.GetRunningDirectory() + "icon.png");
}
else
{
IconFile = MissionPlanner.Properties.Resources.mpdesktop.ToBitmap();
}
if (File.Exists(Settings.GetRunningDirectory() + "splashbg.png")) // 600*375
{
SplashBG = new Bitmap(Settings.GetRunningDirectory() + "splashbg.png");
}
Splash = new MissionPlanner.Splash();
if (SplashBG != null)
{
Splash.BackgroundImage = SplashBG;
Splash.pictureBox1.Visible = false;
}
if (IconFile != null)
{
Splash.Icon = Icon.FromHandle(((Bitmap)IconFile).GetHicon());
}
string strVersion = File.Exists("version.txt")
? File.ReadAllText("version.txt")
: System.Reflection.Assembly.GetExecutingAssembly().GetName().Version.ToString();
Splash.Text = name + " " + Application.ProductVersion + " build " + strVersion;
Splash.Show();
Application.DoEvents();
Application.DoEvents();
CustomMessageBox.ShowEvent += (text, caption, buttons, icon) =>
{
return((CustomMessageBox.DialogResult)(int) MsgBox.CustomMessageBox.Show(text, caption, (MessageBoxButtons)(int)buttons, (MessageBoxIcon)(int)icon));
};
// setup theme provider
MsgBox.CustomMessageBox.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo;
Controls.MainSwitcher.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo;
MissionPlanner.Controls.InputBox.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo;
Controls.BackstageView.BackstageViewPage.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo;
Controls.MainSwitcher.Tracking += MissionPlanner.Utilities.Tracking.AddPage;
Controls.BackstageView.BackstageView.Tracking += MissionPlanner.Utilities.Tracking.AddPage;
// setup settings provider
MissionPlanner.Comms.CommsBase.Settings += CommsBase_Settings;
MissionPlanner.Comms.CommsBase.InputBoxShow += CommsBaseOnInputBoxShow;
MissionPlanner.Comms.CommsBase.ApplyTheme += MissionPlanner.Utilities.ThemeManager.ApplyThemeTo;
// set the cache provider to my custom version
GMap.NET.GMaps.Instance.PrimaryCache = new Maps.MyImageCache();
// add my custom map providers
GMap.NET.MapProviders.GMapProviders.List.Add(Maps.WMSProvider.Instance);
GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Custom.Instance);
GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Earthbuilder.Instance);
GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Statkart_Topo2.Instance);
GMap.NET.MapProviders.GMapProviders.List.Add(Maps.Eniro_Topo.Instance);
GMap.NET.MapProviders.GMapProviders.List.Add(Maps.MapBox.Instance);
GMap.NET.MapProviders.GMapProviders.List.Add(Maps.MapboxNoFly.Instance);
// optionally add gdal support
if (Directory.Exists(Application.StartupPath + Path.DirectorySeparatorChar + "gdal"))
{
GMap.NET.MapProviders.GMapProviders.List.Add(GDAL.GDALProvider.Instance);
}
// add proxy settings
GMap.NET.MapProviders.GMapProvider.WebProxy = WebRequest.GetSystemWebProxy();
GMap.NET.MapProviders.GMapProvider.WebProxy.Credentials = CredentialCache.DefaultCredentials;
// generic status report screen
MAVLinkInterface.CreateIProgressReporterDialogue += title =>
new ProgressReporterDialogue()
{
StartPosition = FormStartPosition.CenterScreen, Text = title
};
WebRequest.DefaultWebProxy = WebRequest.GetSystemWebProxy();
WebRequest.DefaultWebProxy.Credentials = CredentialCache.DefaultNetworkCredentials;
if (name == "VVVVZ")
{
// set pw
Settings.Instance["password"] = "******";
Settings.Instance["password_protect"] = "True";
// prevent wizard
Settings.Instance["newuser"] = "******";
// invalidate update url
System.Configuration.ConfigurationManager.AppSettings["UpdateLocationVersion"] = "";
}
CleanupFiles();
log.InfoFormat("64bit os {0}, 64bit process {1}", System.Environment.Is64BitOperatingSystem,
System.Environment.Is64BitProcess);
Device.DeviceStructure test1 = new Device.DeviceStructure(73225);
Device.DeviceStructure test2 = new Device.DeviceStructure(262434);
Device.DeviceStructure test3 = new Device.DeviceStructure(131874);
//ph2 - cube with here
Device.DeviceStructure test5 = new Device.DeviceStructure(466441);
Device.DeviceStructure test6 = new Device.DeviceStructure(131874);
Device.DeviceStructure test7 = new Device.DeviceStructure(263178);
//
Device.DeviceStructure test8 = new Device.DeviceStructure(1442082);
Device.DeviceStructure test9 = new Device.DeviceStructure(1114914);
Device.DeviceStructure test10 = new Device.DeviceStructure(1442826);
//
Device.DeviceStructure test11 = new Device.DeviceStructure(2359586);
Device.DeviceStructure test12 = new Device.DeviceStructure(2229282);
Device.DeviceStructure test13 = new Device.DeviceStructure(2360330);
MAVLink.MavlinkParse tmp = new MAVLink.MavlinkParse();
MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t()
{
autopilot = 1,
base_mode = 2,
custom_mode = 3,
mavlink_version = 2,
system_status = 6,
type = 7
};
var t1 = tmp.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);
var t2 = tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);
tmp.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);
tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);
tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb, true);
tmp.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb, true);
var msg = new MAVLink.MAVLinkMessage(t2);
try
{
//System.Diagnostics.Process.GetCurrentProcess().PriorityClass = System.Diagnostics.ProcessPriorityClass.RealTime;
Thread.CurrentThread.Name = "Base Thread";
Application.Run(new MainV2());
}
catch (Exception ex)
{
log.Fatal("Fatal app exception", ex);
Console.WriteLine(ex.ToString());
Console.WriteLine("\nPress any key to exit!");
Console.ReadLine();
}
try
{
// kill sim background process if its still running
if (Controls.SITL.simulator != null)
{
Controls.SITL.simulator.Kill();
}
}
catch
{
}
}
public static void tlog(string logfile)
{
List <MLArray> mlList = new List <MLArray>();
Hashtable datappl = new Hashtable();
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch
{
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return;
}
mine.logreadmode = true;
mine.speechenabled = false;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
MAVLink.MAVLinkMessage packet = mine.readPacket();
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 = mine.lastlogread;
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
{
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
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, true);
}
catch (Exception err)
{
MessageBox.Show("There was an error when creating the MAT-file: \n" + err.ToString(),
"MAT-File Creation Error!", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
return;
}
}
public static void anonymise(string logfile, string outputfile)
{
if (!File.Exists(logfile))
{
return;
}
var rand = new Random();
var latrandom = 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"
};
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 + latrandom * 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 + latrandom);
}
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");
if (index != -1)
{
var lat = Double.Parse(dfItem.items[index], CultureInfo.InvariantCulture);
if (lat != 0)
{
dfItem.items[index] = (lat + latrandom).ToString(CultureInfo.InvariantCulture);
}
}
var str = String.Join(",", dfItem.items) + "\r\n";
outfilestream.Write(ASCIIEncoding.ASCII.GetBytes(str), 0, str.Length);
}
}
}
}
void dolog()
{
flightdata.Clear();
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(txt_tlog.Text, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch
{
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return;
}
mine.logreadmode = true;
mine.readPacket();
startlogtime = mine.lastlogread;
double oldlatlngsum = 0;
int appui = 0;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
MAVLink.MAVLinkMessage packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
if (appui != DateTime.Now.Second)
{
// cant do entire app as mixes with flightdata timer
this.Refresh();
appui = DateTime.Now.Second;
}
try
{
if (MainV2.speechEngine != null)
{
MainV2.speechEngine.SpeakAsyncCancelAll();
}
}
catch
{
}
// ignore because of this Exception System.PlatformNotSupportedException: No voice installed on the system or none available with the current security setting.
// if ((float)(cs.lat + cs.lng + cs.alt) != oldlatlngsum
// && cs.lat != 0 && cs.lng != 0)
DateTime nexttime = mine.lastlogread.AddMilliseconds(-(mine.lastlogread.Millisecond % 100));
if (!flightdata.ContainsKey(nexttime))
{
Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " +
(float)(cs.lat + cs.lng + cs.alt) + "!=" + oldlatlngsum);
CurrentState cs2 = (CurrentState)cs.Clone();
try
{
flightdata.Add(nexttime, cs2);
}
catch
{
}
oldlatlngsum = (cs.lat + cs.lng + cs.alt);
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
}
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);
if (packet == null)
{
continue;
}
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);
}
}
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 (
DFLogBuffer colbuf =
new DFLogBuffer(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");
}
}
}
