mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-20 15:48:29 -04:00
39640e8d94
prep for ac2 2.0.43 - simple mode modify some scaling in Config add hud speed warning. add link quality and time to HUD fix ac2 logs, relative alt. prep for mavlink 1.0 add time to tlog > plain text conversion
2008 lines
72 KiB
C#
2008 lines
72 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Text;
|
|
using System.IO.Ports;
|
|
using System.Runtime.InteropServices;
|
|
using System.Collections; // hashs
|
|
using System.Diagnostics; // stopwatch
|
|
using System.Reflection;
|
|
using System.Reflection.Emit;
|
|
using System.IO;
|
|
|
|
|
|
namespace ArdupilotMega
|
|
{
|
|
public partial class MAVLink
|
|
{
|
|
public ICommsSerial BaseStream = new SerialPort();
|
|
|
|
/// <summary>
|
|
/// used for outbound packet sending
|
|
/// </summary>
|
|
byte packetcount = 0;
|
|
public byte sysid = 0;
|
|
public byte compid = 0;
|
|
public Hashtable param = new Hashtable();
|
|
public static byte[][] packets = new byte[256][];
|
|
public double[] packetspersecond = new double[256];
|
|
DateTime[] packetspersecondbuild = new DateTime[256];
|
|
static object objlock = new object();
|
|
static object readlock = new object();
|
|
object logwritelock = new object();
|
|
public DateTime lastvalidpacket = DateTime.Now;
|
|
bool oldlogformat = false;
|
|
|
|
byte mavlinkversion = 0;
|
|
|
|
public PointLatLngAlt[] wps = new PointLatLngAlt[200];
|
|
|
|
public bool debugmavlink = false;
|
|
|
|
public bool logreadmode = false;
|
|
public DateTime lastlogread = DateTime.MinValue;
|
|
public BinaryReader logplaybackfile = null;
|
|
public BinaryWriter logfile = null;
|
|
|
|
public static byte[] streams = new byte[256];
|
|
|
|
int bps1 = 0;
|
|
int bps2 = 0;
|
|
public int bps = 0;
|
|
public DateTime bpstime = DateTime.Now;
|
|
int recvpacketcount = 0;
|
|
|
|
float synclost;
|
|
float packetslost = 0;
|
|
float packetsnotlost = 0;
|
|
DateTime packetlosttimer = DateTime.Now;
|
|
|
|
|
|
//Stopwatch stopwatch = new Stopwatch();
|
|
|
|
public void Close()
|
|
{
|
|
BaseStream.Close();
|
|
}
|
|
|
|
public void Open()
|
|
{
|
|
Open(false);
|
|
}
|
|
|
|
public void Open(bool getparams)
|
|
{
|
|
if (BaseStream.IsOpen)
|
|
return;
|
|
|
|
System.Windows.Forms.Form frm = Common.LoadingBox("Mavlink Connecting..", "Mavlink Connecting..");
|
|
frm.TopMost = true;
|
|
|
|
// reset
|
|
sysid = 0;
|
|
compid = 0;
|
|
param = new Hashtable();
|
|
|
|
try
|
|
{
|
|
MainV2.givecomport = true;
|
|
|
|
BaseStream.ReadBufferSize = 4 * 1024;
|
|
|
|
lock (objlock) // so we dont have random traffic
|
|
{
|
|
|
|
BaseStream.Open();
|
|
|
|
BaseStream.DiscardInBuffer();
|
|
|
|
System.Threading.Thread.Sleep(200); // allow reset to work
|
|
|
|
if (BaseStream.DtrEnable)
|
|
BaseStream.DtrEnable = false;
|
|
|
|
// allow 2560 connect timeout on usb
|
|
System.Threading.Thread.Sleep(1000);
|
|
|
|
}
|
|
|
|
byte[] buffer;
|
|
byte[] buffer1;
|
|
|
|
DateTime start = DateTime.Now;
|
|
|
|
int count = 0;
|
|
|
|
while (true)
|
|
{
|
|
System.Windows.Forms.Application.DoEvents();
|
|
|
|
// incase we are in setup mode
|
|
BaseStream.WriteLine("planner\rgcs\r");
|
|
|
|
frm.Controls[0].Text = (start.AddSeconds(30) - DateTime.Now).Seconds.ToString("Timeout in 0");
|
|
|
|
if (lastbad[0] == '!' && lastbad[1] == 'G' || lastbad[0] == 'G' && lastbad[1] == '!') // waiting for gps lock
|
|
{
|
|
frm.Controls[0].Text = "Waiting for GPS detection..";
|
|
start = start.AddSeconds(5); // each round is 1.1 seconds
|
|
}
|
|
|
|
System.Windows.Forms.Application.DoEvents();
|
|
|
|
if (!(start.AddSeconds(30) > DateTime.Now))
|
|
{
|
|
/*
|
|
System.Windows.Forms.DialogResult dr = System.Windows.Forms.MessageBox.Show("Data recevied but no mavlink packets where read from this port\nWhat do you want to do",
|
|
"Read Fail", System.Windows.Forms.MessageBoxButtons.RetryCancel);
|
|
if (dr == System.Windows.Forms.DialogResult.Retry)
|
|
{
|
|
port.toggleDTRnow(); // force reset on usb
|
|
start = DateTime.Now;
|
|
}
|
|
else*/
|
|
{
|
|
frm.Close();
|
|
this.Close();
|
|
throw new Exception("No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nIt might also be waiting for GPS Lock\nAPM Planner waits for 2 valid heartbeat packets before connecting");
|
|
}
|
|
}
|
|
|
|
System.Threading.Thread.Sleep(1);
|
|
|
|
// incase we are in setup mode
|
|
BaseStream.WriteLine("planner\rgcs\r");
|
|
|
|
System.Windows.Forms.Application.DoEvents();
|
|
|
|
buffer = getHeartBeat();
|
|
|
|
System.Windows.Forms.Application.DoEvents();
|
|
|
|
// incase we are in setup mode
|
|
BaseStream.WriteLine("planner\rgcs\r");
|
|
|
|
System.Threading.Thread.Sleep(1);
|
|
|
|
System.Windows.Forms.Application.DoEvents();
|
|
|
|
buffer1 = getHeartBeat();
|
|
|
|
System.Windows.Forms.Application.DoEvents();
|
|
|
|
try
|
|
{
|
|
Console.WriteLine("MAv Data: len " + buffer.Length + " btr " + BaseStream.BytesToRead);
|
|
}
|
|
catch { }
|
|
|
|
count++;
|
|
|
|
if (buffer.Length > 5 && buffer1.Length > 5 && buffer[3] == buffer1[3] && buffer[4] == buffer1[4])
|
|
{
|
|
__mavlink_heartbeat_t hb = new __mavlink_heartbeat_t();
|
|
|
|
object temp = hb;
|
|
|
|
MAVLink.ByteArrayToStructure(buffer, ref temp, 6);
|
|
|
|
hb = (MAVLink.__mavlink_heartbeat_t)(temp);
|
|
|
|
mavlinkversion = hb.mavlink_version;
|
|
|
|
sysid = buffer[3];
|
|
compid = buffer[4];
|
|
recvpacketcount = buffer[2];
|
|
Console.WriteLine("ID sys " + sysid + " comp " + compid + " ver" + mavlinkversion);
|
|
break;
|
|
}
|
|
}
|
|
|
|
frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + " compid "+compid+") ";
|
|
frm.Refresh();
|
|
if (getparams == true)
|
|
getParamList();
|
|
}
|
|
catch (Exception e) { MainV2.givecomport = false; frm.Close(); throw e; }
|
|
|
|
frm.Close();
|
|
|
|
MainV2.givecomport = false;
|
|
|
|
Console.WriteLine("Done open " + sysid + " " + compid);
|
|
|
|
}
|
|
|
|
public static byte[] StructureToByteArrayEndian(params object[] list)
|
|
{
|
|
// The copy is made becuase SetValue won't work on a struct.
|
|
// Boxing was used because SetValue works on classes/objects.
|
|
// Unfortunately, it results in 2 copy operations.
|
|
object thisBoxed = list[0]; // Why make a copy?
|
|
Type test = thisBoxed.GetType();
|
|
|
|
int offset = 0;
|
|
byte[] data = new byte[Marshal.SizeOf(thisBoxed)];
|
|
|
|
// System.Net.IPAddress.NetworkToHostOrder is used to perform byte swapping.
|
|
// To convert unsigned to signed, 'unchecked()' was used.
|
|
// See http://stackoverflow.com/questions/1131843/how-do-i-convert-uint-to-int-in-c
|
|
|
|
object fieldValue;
|
|
TypeCode typeCode;
|
|
|
|
byte[] temp;
|
|
|
|
// Enumerate each structure field using reflection.
|
|
foreach (var field in test.GetFields())
|
|
{
|
|
// field.Name has the field's name.
|
|
|
|
fieldValue = field.GetValue(thisBoxed); // Get value
|
|
|
|
// Get the TypeCode enumeration. Multiple types get mapped to a common typecode.
|
|
typeCode = Type.GetTypeCode(fieldValue.GetType());
|
|
|
|
switch (typeCode)
|
|
{
|
|
case TypeCode.Single: // float
|
|
{
|
|
temp = BitConverter.GetBytes((Single)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(Single));
|
|
break;
|
|
}
|
|
case TypeCode.Int32:
|
|
{
|
|
temp = BitConverter.GetBytes((Int32)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(Int32));
|
|
break;
|
|
}
|
|
case TypeCode.UInt32:
|
|
{
|
|
temp = BitConverter.GetBytes((UInt32)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(UInt32));
|
|
break;
|
|
}
|
|
case TypeCode.Int16:
|
|
{
|
|
temp = BitConverter.GetBytes((Int16)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(Int16));
|
|
break;
|
|
}
|
|
case TypeCode.UInt16:
|
|
{
|
|
temp = BitConverter.GetBytes((UInt16)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(UInt16));
|
|
break;
|
|
}
|
|
case TypeCode.Int64:
|
|
{
|
|
temp = BitConverter.GetBytes((Int64)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(Int64));
|
|
break;
|
|
}
|
|
case TypeCode.UInt64:
|
|
{
|
|
temp = BitConverter.GetBytes((UInt64)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(UInt64));
|
|
break;
|
|
}
|
|
case TypeCode.Double:
|
|
{
|
|
temp = BitConverter.GetBytes((Double)fieldValue);
|
|
Array.Reverse(temp);
|
|
Array.Copy(temp, 0, data, offset, sizeof(Double));
|
|
break;
|
|
}
|
|
case TypeCode.Byte:
|
|
{
|
|
data[offset] = (Byte)fieldValue;
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
//System.Diagnostics.Debug.Fail("No conversion provided for this type : " + typeCode.ToString());
|
|
break;
|
|
}
|
|
}; // switch
|
|
if (typeCode == TypeCode.Object)
|
|
{
|
|
int length = ((byte[])fieldValue).Length;
|
|
Array.Copy(((byte[])fieldValue), 0, data, offset, length);
|
|
offset += length;
|
|
}
|
|
else
|
|
{
|
|
offset += Marshal.SizeOf(fieldValue);
|
|
}
|
|
} // foreach
|
|
|
|
return data;
|
|
} // Swap
|
|
|
|
byte[] getHeartBeat()
|
|
{
|
|
DateTime start = DateTime.Now;
|
|
while (true)
|
|
{
|
|
byte[] buffer = readPacket();
|
|
if (buffer.Length > 5)
|
|
{
|
|
if (buffer[5] == MAVLINK_MSG_ID_HEARTBEAT)
|
|
{
|
|
return buffer;
|
|
}
|
|
}
|
|
if (DateTime.Now > start.AddMilliseconds(2200)) // was 1200 , now 2.2 sec
|
|
return new byte[0];
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Generate a Mavlink Packet and write to serial
|
|
/// </summary>
|
|
/// <param name="messageType">type number</param>
|
|
/// <param name="indata">struct of data</param>
|
|
public void generatePacket(byte messageType, object indata)
|
|
{
|
|
byte[] data;
|
|
|
|
if (mavlinkversion == 3)
|
|
{
|
|
data = StructureToByteArray(indata);
|
|
}
|
|
else
|
|
{
|
|
data = StructureToByteArrayEndian(indata);
|
|
}
|
|
|
|
//Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
|
|
byte[] packet = new byte[data.Length + 6 + 2];
|
|
|
|
if (mavlinkversion == 3)
|
|
{
|
|
packet[0] = 254;
|
|
}
|
|
else if (mavlinkversion == 2)
|
|
{
|
|
packet[0] = (byte)'U';
|
|
}
|
|
packet[1] = (byte)data.Length;
|
|
packet[2] = packetcount;
|
|
packet[3] = 255; // this is always 255 - MYGCS
|
|
packet[4] = (byte)MAV_COMPONENT.MAV_COMP_ID_WAYPOINTPLANNER;
|
|
packet[5] = messageType;
|
|
|
|
int i = 6;
|
|
foreach (byte b in data)
|
|
{
|
|
packet[i] = b;
|
|
i++;
|
|
}
|
|
|
|
ushort checksum = crc_calculate(packet, packet[1] + 6);
|
|
|
|
if (mavlinkversion == 3)
|
|
{
|
|
checksum = crc_accumulate(MAVLINK_MESSAGE_CRCS[messageType], checksum);
|
|
}
|
|
|
|
byte ck_a = (byte)(checksum & 0xFF); ///< High byte
|
|
byte ck_b = (byte)(checksum >> 8); ///< Low byte
|
|
|
|
packet[i] = ck_a;
|
|
i += 1;
|
|
packet[i] = ck_b;
|
|
i += 1;
|
|
|
|
if (BaseStream.IsOpen)
|
|
{
|
|
lock (objlock)
|
|
{
|
|
BaseStream.Write(packet, 0, i);
|
|
}
|
|
}
|
|
|
|
try
|
|
{
|
|
if (logfile != null)
|
|
{
|
|
lock (logwritelock)
|
|
{
|
|
byte[] datearray = BitConverter.GetBytes((UInt64)((DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds * 1000)); //ASCIIEncoding.ASCII.GetBytes(DateTime.Now.ToBinary() + ":");
|
|
Array.Reverse(datearray);
|
|
logfile.Write(datearray, 0, datearray.Length);
|
|
logfile.Write(packet, 0, i);
|
|
}
|
|
}
|
|
|
|
}
|
|
catch { }
|
|
|
|
if (messageType == ArdupilotMega.MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM)
|
|
{
|
|
try
|
|
{
|
|
BinaryWriter bw = new BinaryWriter(File.OpenWrite("serialsent.raw"));
|
|
bw.Seek(0, SeekOrigin.End);
|
|
bw.Write(packet, 0, i);
|
|
bw.Write((byte)'\n');
|
|
bw.Close();
|
|
}
|
|
catch { } // been getting errors from this. people must have it open twice.
|
|
}
|
|
|
|
packetcount++;
|
|
|
|
//System.Threading.Thread.Sleep(1);
|
|
}
|
|
|
|
public bool Write(string line)
|
|
{
|
|
lock (objlock)
|
|
{
|
|
BaseStream.Write(line);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Set parameter on apm
|
|
/// </summary>
|
|
/// <param name="paramname">name as a string</param>
|
|
/// <param name="value"></param>
|
|
public bool setParam(string paramname, float value)
|
|
{
|
|
MainV2.givecomport = true;
|
|
|
|
__mavlink_param_set_t req = new __mavlink_param_set_t();
|
|
req.target_system = sysid;
|
|
req.target_component = compid;
|
|
|
|
byte[] temp = ASCIIEncoding.ASCII.GetBytes(paramname);
|
|
|
|
modifyParamForDisplay(false, paramname, ref value);
|
|
|
|
Array.Resize(ref temp, 15);
|
|
|
|
req.param_id = temp;
|
|
req.param_value = (value);
|
|
|
|
generatePacket(MAVLINK_MSG_ID_PARAM_SET, req);
|
|
|
|
Console.WriteLine("setParam '{0}' = '{1}' sysid {2} compid {3}", paramname, req.param_value, sysid, compid);
|
|
|
|
DateTime start = DateTime.Now;
|
|
int retrys = 3;
|
|
|
|
while (true)
|
|
{
|
|
if (!(start.AddMilliseconds(500) > DateTime.Now))
|
|
{
|
|
if (retrys > 0)
|
|
{
|
|
Console.WriteLine("setParam Retry " + retrys);
|
|
generatePacket(MAVLINK_MSG_ID_PARAM_SET, req);
|
|
start = DateTime.Now;
|
|
retrys--;
|
|
continue;
|
|
}
|
|
MainV2.givecomport = false;
|
|
throw new Exception("Timeout on read - setParam " + paramname);
|
|
}
|
|
|
|
byte[] buffer = readPacket();
|
|
if (buffer.Length > 5)
|
|
{
|
|
if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE)
|
|
{
|
|
__mavlink_param_value_t par = new __mavlink_param_value_t();
|
|
|
|
object tempobj = par;
|
|
|
|
ByteArrayToStructure(buffer, ref tempobj, 6);
|
|
|
|
par = (__mavlink_param_value_t)tempobj;
|
|
|
|
string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id);
|
|
|
|
int pos = st.IndexOf('\0');
|
|
|
|
if (pos != -1)
|
|
{
|
|
st = st.Substring(0, pos);
|
|
}
|
|
|
|
if (st != paramname)
|
|
{
|
|
Console.WriteLine("MAVLINK bad param responce - {0} vs {1}",paramname,st);
|
|
continue;
|
|
}
|
|
|
|
param[st] = (par.param_value);
|
|
|
|
MainV2.givecomport = false;
|
|
//System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public struct _param
|
|
{
|
|
public string name;
|
|
public float value;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get param list from apm
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public Hashtable getParamList()
|
|
{
|
|
MainV2.givecomport = true;
|
|
List<int> missed = new List<int>();
|
|
_param[] paramarray = new _param[300];
|
|
|
|
__mavlink_param_request_list_t req = new __mavlink_param_request_list_t();
|
|
req.target_system = sysid;
|
|
req.target_component = compid;
|
|
|
|
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req);
|
|
|
|
DateTime start = DateTime.Now;
|
|
|
|
int retrys = 3;
|
|
int nextid = 0;
|
|
int a = 0;
|
|
int z = 5;
|
|
while (a < z)
|
|
{
|
|
if (!(start.AddMilliseconds(5000) > DateTime.Now))
|
|
{
|
|
if (retrys > 0)
|
|
{
|
|
Console.WriteLine("getParamList Retry " + retrys + " sys " + sysid + " comp " + compid);
|
|
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req);
|
|
start = DateTime.Now;
|
|
retrys--;
|
|
continue;
|
|
}
|
|
MainV2.givecomport = false;
|
|
throw new Exception("Timeout on read - getParamList");
|
|
}
|
|
byte[] buffer = readPacket();
|
|
if (buffer.Length > 5)
|
|
{
|
|
//stopwatch.Reset();
|
|
//stopwatch.Start();
|
|
if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE)
|
|
{
|
|
start = DateTime.Now;
|
|
|
|
__mavlink_param_value_t par = new __mavlink_param_value_t();
|
|
|
|
object temp = par;
|
|
|
|
ByteArrayToStructure(buffer, ref temp, 6);
|
|
|
|
par = (__mavlink_param_value_t)temp;
|
|
|
|
z = (par.param_count);
|
|
|
|
Console.WriteLine(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (z - 1));
|
|
|
|
if (nextid == (par.param_index))
|
|
{
|
|
nextid++;
|
|
}
|
|
else
|
|
{
|
|
if (retrys > 0)
|
|
{
|
|
generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req);
|
|
a = 0;
|
|
nextid = 0;
|
|
retrys--;
|
|
continue;
|
|
}
|
|
missed.Add(nextid); // for later devel
|
|
MainV2.givecomport = false;
|
|
throw new Exception("Missed ID expecting " + nextid + " got " + (par.param_index) + "\nPlease try loading again");
|
|
}
|
|
|
|
string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id);
|
|
|
|
int pos = st.IndexOf('\0');
|
|
|
|
if (pos != -1)
|
|
{
|
|
st = st.Substring(0, pos);
|
|
}
|
|
|
|
modifyParamForDisplay(true, st, ref par.param_value);
|
|
|
|
param[st] = (par.param_value);
|
|
|
|
a++;
|
|
}
|
|
else
|
|
{
|
|
//Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " " + this.BytesToRead);
|
|
}
|
|
//stopwatch.Stop();
|
|
//Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed);
|
|
}
|
|
}
|
|
MainV2.givecomport = false;
|
|
return param;
|
|
}
|
|
|
|
void modifyParamForDisplay(bool fromapm, string paramname, ref float value)
|
|
{
|
|
if (paramname.ToUpper().EndsWith("_IMAX") || paramname.ToUpper().EndsWith("ALT_HOLD_RTL") || paramname.ToUpper().EndsWith("TRIM_ARSPD_CM")
|
|
|| paramname.ToUpper().EndsWith("XTRK_ANGLE_CD") || paramname.ToUpper().EndsWith("LIM_PITCH_MAX") || paramname.ToUpper().EndsWith("LIM_PITCH_MIN")
|
|
|| paramname.ToUpper().EndsWith("LIM_ROLL_CD") || paramname.ToUpper().EndsWith("PITCH_MAX") || paramname.ToUpper().EndsWith("WP_SPEED_MAX"))
|
|
{
|
|
if (fromapm)
|
|
{
|
|
value /= 100.0f;
|
|
}
|
|
else
|
|
{
|
|
value *= 100.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stops all requested data packets.
|
|
/// </summary>
|
|
public void stopall(bool forget)
|
|
{
|
|
__mavlink_request_data_stream_t req = new __mavlink_request_data_stream_t();
|
|
req.target_system = sysid;
|
|
req.target_component = compid;
|
|
|
|
req.req_message_rate = 10;
|
|
req.start_stop = 0; // stop
|
|
req.req_stream_id = 0; // all
|
|
|
|
// reset all
|
|
if (forget)
|
|
{
|
|
lock (objlock)
|
|
{
|
|
streams = new byte[streams.Length];
|
|
}
|
|
}
|
|
|
|
// no error on bad
|
|
try
|
|
{
|
|
generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
|
|
System.Threading.Thread.Sleep(20);
|
|
generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
|
|
System.Threading.Thread.Sleep(20);
|
|
generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
|
|
Console.WriteLine("Stopall Done");
|
|
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
public void setWPACK()
|
|
{
|
|
MAVLink.__mavlink_waypoint_ack_t req = new MAVLink.__mavlink_waypoint_ack_t();
|
|
req.target_system = sysid;
|
|
req.target_component = compid;
|
|
req.type = 0;
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_ACK, req);
|
|
}
|
|
|
|
public bool setWPCurrent(ushort index)
|
|
{
|
|
MainV2.givecomport = true;
|
|
byte[] buffer;
|
|
|
|
__mavlink_waypoint_set_current_t req = new __mavlink_waypoint_set_current_t();
|
|
|
|
req.target_system = sysid;
|
|
req.target_component = compid;
|
|
req.seq = index;
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req);
|
|
|
|
DateTime start = DateTime.Now;
|
|
int retrys = 5;
|
|
|
|
while (true)
|
|
{
|
|
if (!(start.AddMilliseconds(2000) > DateTime.Now))
|
|
{
|
|
if (retrys > 0)
|
|
{
|
|
Console.WriteLine("setWPCurrent Retry " + retrys);
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, req);
|
|
start = DateTime.Now;
|
|
retrys--;
|
|
continue;
|
|
}
|
|
MainV2.givecomport = false;
|
|
throw new Exception("Timeout on read - setWPCurrent");
|
|
}
|
|
|
|
buffer = readPacket();
|
|
if (buffer.Length > 5)
|
|
{
|
|
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_CURRENT)
|
|
{
|
|
MainV2.givecomport = false;
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public bool doAction(MAV_ACTION actionid)
|
|
{
|
|
MainV2.givecomport = true;
|
|
byte[] buffer;
|
|
|
|
__mavlink_action_t req = new __mavlink_action_t();
|
|
|
|
req.target = sysid;
|
|
req.target_component = compid;
|
|
|
|
req.action = (byte)actionid;
|
|
|
|
generatePacket(MAVLINK_MSG_ID_ACTION, req);
|
|
|
|
DateTime start = DateTime.Now;
|
|
int retrys = 3;
|
|
|
|
int timeout = 2000;
|
|
|
|
// imu calib take a little while
|
|
if (actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_ACC ||
|
|
actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_GYRO ||
|
|
actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_MAG ||
|
|
actionid == MAV_ACTION.MAV_ACTION_CALIBRATE_PRESSURE ||
|
|
actionid == MAV_ACTION.MAV_ACTION_REBOOT)
|
|
{
|
|
retrys = 1;
|
|
timeout = 6000;
|
|
}
|
|
|
|
while (true)
|
|
{
|
|
if (!(start.AddMilliseconds(timeout) > DateTime.Now))
|
|
{
|
|
if (retrys > 0)
|
|
{
|
|
Console.WriteLine("doAction Retry " + retrys);
|
|
generatePacket(MAVLINK_MSG_ID_ACTION, req);
|
|
start = DateTime.Now;
|
|
retrys--;
|
|
continue;
|
|
}
|
|
MainV2.givecomport = false;
|
|
throw new Exception("Timeout on read - doAction");
|
|
}
|
|
|
|
buffer = readPacket();
|
|
if (buffer.Length > 5)
|
|
{
|
|
if (buffer[5] == MAVLINK_MSG_ID_ACTION_ACK)
|
|
{
|
|
if (buffer[7] == 1)
|
|
{
|
|
MainV2.givecomport = false;
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
MainV2.givecomport = false;
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public void requestDatastream(byte id, byte hzrate)
|
|
{
|
|
lock (objlock)
|
|
{
|
|
streams[id] = hzrate;
|
|
}
|
|
|
|
double pps = 0;
|
|
|
|
switch (id)
|
|
{
|
|
case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_ALL:
|
|
|
|
break;
|
|
case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_EXTENDED_STATUS:
|
|
if (packetspersecondbuild[MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2))
|
|
break;
|
|
pps = packetspersecond[MAVLINK_MSG_ID_SYS_STATUS];
|
|
if (hzratecheck(pps, hzrate))
|
|
{
|
|
return;
|
|
}
|
|
break;
|
|
case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_EXTRA1:
|
|
if (packetspersecondbuild[MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2))
|
|
break;
|
|
pps = packetspersecond[MAVLINK_MSG_ID_ATTITUDE];
|
|
if (hzratecheck(pps, hzrate))
|
|
{
|
|
return;
|
|
}
|
|
break;
|
|
case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_EXTRA2:
|
|
if (packetspersecondbuild[MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2))
|
|
break;
|
|
pps = packetspersecond[MAVLINK_MSG_ID_VFR_HUD];
|
|
if (hzratecheck(pps, hzrate))
|
|
{
|
|
return;
|
|
}
|
|
break;
|
|
case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_EXTRA3:
|
|
|
|
break;
|
|
case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_POSITION:
|
|
if (packetspersecondbuild[MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2))
|
|
break;
|
|
pps = packetspersecond[MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
|
|
if (hzratecheck(pps, hzrate))
|
|
{
|
|
return;
|
|
}
|
|
break;
|
|
case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RAW_CONTROLLER:
|
|
if (packetspersecondbuild[MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2))
|
|
break;
|
|
pps = packetspersecond[MAVLINK_MSG_ID_RC_CHANNELS_SCALED];
|
|
if (hzratecheck(pps, hzrate))
|
|
{
|
|
return;
|
|
}
|
|
break;
|
|
case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RAW_SENSORS:
|
|
if (packetspersecondbuild[MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2))
|
|
break;
|
|
pps = packetspersecond[MAVLINK_MSG_ID_RAW_IMU];
|
|
if (hzratecheck(pps, hzrate))
|
|
{
|
|
return;
|
|
}
|
|
break;
|
|
case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RC_CHANNELS:
|
|
if (packetspersecondbuild[MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2))
|
|
break;
|
|
pps = packetspersecond[MAVLINK_MSG_ID_RC_CHANNELS_RAW];
|
|
if (hzratecheck(pps, hzrate))
|
|
{
|
|
return;
|
|
}
|
|
break;
|
|
}
|
|
|
|
//packetspersecond[temp[5]];
|
|
|
|
if (pps == 0 && hzrate == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
Console.WriteLine("Request stream {0} at {1} hz : currently {2}", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate, pps);
|
|
getDatastream(id, hzrate);
|
|
}
|
|
|
|
// returns true for ok
|
|
bool hzratecheck(double pps, int hzrate)
|
|
{
|
|
|
|
if (hzrate == 0 && pps == 0)
|
|
{
|
|
return true;
|
|
}
|
|
else if (hzrate == 1 && pps >= 0.5 && pps <= 2)
|
|
{
|
|
return true;
|
|
}
|
|
else if (hzrate == 3 && pps >= 2 && hzrate < 5)
|
|
{
|
|
return true;
|
|
}
|
|
else if (hzrate == 10 && pps > 5 && hzrate < 15)
|
|
{
|
|
return true;
|
|
}
|
|
else if (hzrate > 15 && pps > 15)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
void getDatastream(byte id, byte hzrate)
|
|
{
|
|
__mavlink_request_data_stream_t req = new __mavlink_request_data_stream_t();
|
|
req.target_system = sysid;
|
|
req.target_component = compid;
|
|
|
|
req.req_message_rate = hzrate;
|
|
req.start_stop = 1; // start
|
|
req.req_stream_id = id; // id
|
|
|
|
generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
|
|
generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns WP count
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public byte getWPCount()
|
|
{
|
|
MainV2.givecomport = true;
|
|
byte[] buffer;
|
|
|
|
__mavlink_waypoint_request_list_t req = new __mavlink_waypoint_request_list_t();
|
|
|
|
req.target_system = sysid;
|
|
req.target_component = compid;
|
|
|
|
// request list
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req);
|
|
|
|
DateTime start = DateTime.Now;
|
|
int retrys = 6;
|
|
|
|
while (true)
|
|
{
|
|
if (!(start.AddMilliseconds(500) > DateTime.Now))
|
|
{
|
|
if (retrys > 0)
|
|
{
|
|
Console.WriteLine("getWPCount Retry " + retrys + " - giv com " + MainV2.givecomport);
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req);
|
|
start = DateTime.Now;
|
|
retrys--;
|
|
continue;
|
|
}
|
|
MainV2.givecomport = false;
|
|
//return (byte)int.Parse(param["WP_TOTAL"].ToString());
|
|
throw new Exception("Timeout on read - getWPCount");
|
|
}
|
|
|
|
buffer = readPacket();
|
|
if (buffer.Length > 5)
|
|
{
|
|
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT)
|
|
{
|
|
|
|
Console.WriteLine("wpcount: " + buffer[9]);
|
|
MainV2.givecomport = false;
|
|
return buffer[9]; // should be ushort, but apm has limited wp count < byte
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine(DateTime.Now + " PC wpcount " + buffer[5] + " need " + MAVLINK_MSG_ID_WAYPOINT_COUNT + " " + this.BaseStream.BytesToRead);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
/// <summary>
|
|
/// Gets specfied WP
|
|
/// </summary>
|
|
/// <param name="index"></param>
|
|
/// <returns>WP</returns>
|
|
public Locationwp getWP(ushort index)
|
|
{
|
|
MainV2.givecomport = true;
|
|
Locationwp loc = new Locationwp();
|
|
|
|
__mavlink_waypoint_request_t req = new __mavlink_waypoint_request_t();
|
|
|
|
req.target_system = sysid;
|
|
req.target_component = compid;
|
|
|
|
req.seq = index;
|
|
|
|
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
|
|
|
|
// request
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req);
|
|
|
|
DateTime start = DateTime.Now;
|
|
int retrys = 5;
|
|
|
|
while (true)
|
|
{
|
|
if (!(start.AddMilliseconds(800) > DateTime.Now)) // apm times out after 1000ms
|
|
{
|
|
if (retrys > 0)
|
|
{
|
|
Console.WriteLine("getWP Retry " + retrys);
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req);
|
|
start = DateTime.Now;
|
|
retrys--;
|
|
continue;
|
|
}
|
|
MainV2.givecomport = false;
|
|
throw new Exception("Timeout on read - getWP");
|
|
}
|
|
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
|
|
byte[] buffer = readPacket();
|
|
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
|
|
if (buffer.Length > 5)
|
|
{
|
|
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT)
|
|
{
|
|
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
|
|
__mavlink_waypoint_t wp = new __mavlink_waypoint_t();
|
|
|
|
object temp = (object)wp;
|
|
|
|
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
|
|
|
|
ByteArrayToStructure(buffer, ref temp, 6);
|
|
|
|
wp = (__mavlink_waypoint_t)(temp);
|
|
|
|
loc.options = (byte)(wp.frame & 0x1);
|
|
loc.id = (byte)(wp.command);
|
|
loc.p1 = (byte)(wp.param1);
|
|
if (loc.id < (byte)MAV_CMD.LAST)
|
|
{
|
|
loc.alt = (int)((wp.z) * 100);
|
|
loc.lat = (int)((wp.x) * 10000000);
|
|
loc.lng = (int)((wp.y) * 10000000);
|
|
}
|
|
else
|
|
{
|
|
|
|
loc.alt = (int)((wp.z));
|
|
loc.lat = (int)((wp.x));
|
|
loc.lng = (int)((wp.y));
|
|
|
|
switch (loc.id)
|
|
{ // Switch to map APM command fields inot MAVLink command fields
|
|
case (byte)MAV_CMD.LOITER_TURNS:
|
|
case (byte)MAV_CMD.TAKEOFF:
|
|
case (byte)MAV_CMD.DO_SET_HOME:
|
|
case (byte)MAV_CMD.DO_SET_ROI:
|
|
loc.alt = (int)((wp.z) * 100);
|
|
loc.lat = (int)((wp.x) * 10000000);
|
|
loc.lng = (int)((wp.y) * 10000000);
|
|
loc.p1 = (byte)wp.param1;
|
|
break;
|
|
|
|
case (byte)MAV_CMD.CONDITION_CHANGE_ALT:
|
|
loc.lat = (int)wp.param1;
|
|
loc.p1 = 0;
|
|
break;
|
|
|
|
case (byte)MAV_CMD.LOITER_TIME:
|
|
if (MainV2.APMFirmware == MainV2.Firmwares.ArduPlane)
|
|
{
|
|
loc.p1 = (byte)(wp.param1 / 10); // APM loiter time is in ten second increments
|
|
}
|
|
else
|
|
{
|
|
loc.p1 = (byte)wp.param1;
|
|
}
|
|
break;
|
|
|
|
case (byte)MAV_CMD.CONDITION_DELAY:
|
|
case (byte)MAV_CMD.CONDITION_DISTANCE:
|
|
loc.lat = (int)wp.param1;
|
|
break;
|
|
|
|
case (byte)MAV_CMD.DO_JUMP:
|
|
loc.lat = (int)wp.param2;
|
|
loc.p1 = (byte)wp.param1;
|
|
break;
|
|
|
|
case (byte)MAV_CMD.DO_REPEAT_SERVO:
|
|
loc.lng = (int)wp.param4;
|
|
goto case (byte)MAV_CMD.DO_CHANGE_SPEED;
|
|
case (byte)MAV_CMD.DO_REPEAT_RELAY:
|
|
case (byte)MAV_CMD.DO_CHANGE_SPEED:
|
|
loc.lat = (int)wp.param3;
|
|
loc.alt = (int)wp.param2;
|
|
loc.p1 = (byte)wp.param1;
|
|
break;
|
|
|
|
case (byte)MAV_CMD.DO_SET_PARAMETER:
|
|
case (byte)MAV_CMD.DO_SET_RELAY:
|
|
case (byte)MAV_CMD.DO_SET_SERVO:
|
|
loc.alt = (int)wp.param2;
|
|
loc.p1 = (byte)wp.param1;
|
|
break;
|
|
|
|
case (byte)MAV_CMD.WAYPOINT:
|
|
loc.p1 = (byte)wp.param1;
|
|
break;
|
|
}
|
|
}
|
|
|
|
Console.WriteLine("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng, loc.options);
|
|
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
|
|
}
|
|
}
|
|
}
|
|
MainV2.givecomport = false;
|
|
return loc;
|
|
}
|
|
|
|
public object DebugPacket(byte[] datin)
|
|
{
|
|
string text = "";
|
|
return DebugPacket(datin, ref text);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Print entire decoded packet to console
|
|
/// </summary>
|
|
/// <param name="datin">packet byte array</param>
|
|
/// <returns>struct of data</returns>
|
|
public object DebugPacket(byte[] datin, ref string text)
|
|
{
|
|
string textoutput;
|
|
try
|
|
{
|
|
if (datin.Length > 5)
|
|
{
|
|
byte header = datin[0];
|
|
byte length = datin[1];
|
|
byte seq = datin[2];
|
|
byte sysid = datin[3];
|
|
byte compid = datin[4];
|
|
byte messid = datin[5];
|
|
|
|
textoutput = string.Format("{0:X} {1:X} {2:X} {3:X} {4:X} {5:X} ", header, length, seq, sysid, compid, messid);
|
|
|
|
object data = Activator.CreateInstance(mavstructs[messid]);
|
|
|
|
ByteArrayToStructure(datin, ref data, 6);
|
|
|
|
Type test = data.GetType();
|
|
|
|
textoutput = textoutput + test.Name + " ";
|
|
|
|
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 + "=";
|
|
byte[] crap = (byte[])fieldValue;
|
|
foreach (byte fiel in crap)
|
|
{
|
|
textoutput = textoutput + fiel + ",";
|
|
}
|
|
textoutput = textoutput + " ";
|
|
}
|
|
else
|
|
{
|
|
textoutput = textoutput + field.Name + "=" + fieldValue.ToString() + " ";
|
|
}
|
|
}
|
|
textoutput = textoutput + " Len:" + datin.Length + "\r\n";
|
|
Console.Write(textoutput);
|
|
|
|
if (text != null)
|
|
text = textoutput;
|
|
|
|
return data;
|
|
}
|
|
}
|
|
catch { }
|
|
|
|
return null;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Sets wp total count
|
|
/// </summary>
|
|
/// <param name="wp_total"></param>
|
|
public void setWPTotal(ushort wp_total)
|
|
{
|
|
MainV2.givecomport = true;
|
|
__mavlink_waypoint_count_t req = new __mavlink_waypoint_count_t();
|
|
|
|
req.target_system = sysid;
|
|
req.target_component = compid; // MAVLINK_MSG_ID_WAYPOINT_COUNT
|
|
|
|
req.count = wp_total;
|
|
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_COUNT, req);
|
|
|
|
DateTime start = DateTime.Now;
|
|
int retrys = 3;
|
|
|
|
while (true)
|
|
{
|
|
if (!(start.AddMilliseconds(700) > DateTime.Now))
|
|
{
|
|
if (retrys > 0)
|
|
{
|
|
Console.WriteLine("setWPTotal Retry " + retrys);
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT_COUNT, req);
|
|
start = DateTime.Now;
|
|
retrys--;
|
|
continue;
|
|
}
|
|
MainV2.givecomport = false;
|
|
throw new Exception("Timeout on read - setWPTotal");
|
|
}
|
|
byte[] buffer = readPacket();
|
|
if (buffer.Length > 9)
|
|
{
|
|
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST && buffer[9] == 0)
|
|
{
|
|
param["WP_TOTAL"] = (float)wp_total - 1;
|
|
MainV2.givecomport = false;
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
//Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Save wp to eeprom
|
|
/// </summary>
|
|
/// <param name="loc">location struct</param>
|
|
/// <param name="index">wp no</param>
|
|
/// <param name="frame">global or relative</param>
|
|
/// <param name="current">0 = no , 2 = guided mode</param>
|
|
public void setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current)
|
|
{
|
|
MainV2.givecomport = true;
|
|
__mavlink_waypoint_t req = new __mavlink_waypoint_t();
|
|
|
|
req.target_system = sysid;
|
|
req.target_component = compid; // MAVLINK_MSG_ID_WAYPOINT
|
|
|
|
req.command = loc.id;
|
|
req.param1 = loc.p1;
|
|
|
|
req.current = current;
|
|
|
|
if (loc.id < (byte)MAV_CMD.LAST)
|
|
{
|
|
req.frame = (byte)frame;
|
|
req.y = (float)(loc.lng / 10000000.0);
|
|
req.x = (float)(loc.lat / 10000000.0);
|
|
req.z = (float)(loc.alt / 100.0);
|
|
}
|
|
else
|
|
{
|
|
req.frame = (byte)MAVLink.MAV_FRAME.MAV_FRAME_MISSION; // mission
|
|
req.y = (float)(loc.lng);
|
|
req.x = (float)(loc.lat);
|
|
req.z = (float)(loc.alt);
|
|
|
|
switch (loc.id)
|
|
{ // Switch to map APM command fields inot MAVLink command fields
|
|
case (byte)MAV_CMD.LOITER_TURNS:
|
|
case (byte)MAV_CMD.TAKEOFF:
|
|
req.param1 = loc.p1;
|
|
break;
|
|
case (byte)MAV_CMD.DO_SET_HOME:
|
|
req.y = (float)(loc.lng / 10000000.0);
|
|
req.x = (float)(loc.lat / 10000000.0);
|
|
req.z = (float)(loc.alt / 100.0);
|
|
req.param1 = loc.p1;
|
|
break;
|
|
|
|
case (byte)MAV_CMD.CONDITION_CHANGE_ALT:
|
|
req.param1 = loc.lat;
|
|
req.x = 0;
|
|
req.y = 0;
|
|
break;
|
|
|
|
case (byte)MAV_CMD.LOITER_TIME:
|
|
req.param1 = loc.p1 * 10; // APM loiter time is in ten second increments
|
|
break;
|
|
|
|
case (byte)MAV_CMD.CONDITION_DELAY:
|
|
case (byte)MAV_CMD.CONDITION_DISTANCE:
|
|
req.param1 = loc.lat;
|
|
break;
|
|
|
|
case (byte)MAV_CMD.DO_JUMP:
|
|
req.param2 = loc.lat;
|
|
req.param1 = loc.p1;
|
|
break;
|
|
|
|
case (byte)MAV_CMD.DO_REPEAT_SERVO:
|
|
req.param4 = loc.lng;
|
|
goto case (byte)MAV_CMD.DO_CHANGE_SPEED;
|
|
case (byte)MAV_CMD.DO_REPEAT_RELAY:
|
|
case (byte)MAV_CMD.DO_CHANGE_SPEED:
|
|
req.param3 = loc.lat;
|
|
req.param2 = loc.alt;
|
|
req.param1 = loc.p1;
|
|
break;
|
|
|
|
case (byte)MAV_CMD.DO_SET_PARAMETER:
|
|
case (byte)MAV_CMD.DO_SET_RELAY:
|
|
case (byte)MAV_CMD.DO_SET_SERVO:
|
|
req.param2 = loc.alt;
|
|
req.param1 = loc.p1;
|
|
break;
|
|
}
|
|
}
|
|
req.seq = index;
|
|
|
|
Console.WriteLine("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index);
|
|
|
|
// request
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
|
|
|
|
DateTime start = DateTime.Now;
|
|
int retrys = 6;
|
|
|
|
while (true)
|
|
{
|
|
if (!(start.AddMilliseconds(500) > DateTime.Now))
|
|
{
|
|
if (retrys > 0)
|
|
{
|
|
Console.WriteLine("setWP Retry " + retrys);
|
|
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
|
|
start = DateTime.Now;
|
|
retrys--;
|
|
continue;
|
|
}
|
|
MainV2.givecomport = false;
|
|
throw new Exception("Timeout on read - setWP");
|
|
}
|
|
byte[] buffer = readPacket();
|
|
if (buffer.Length > 5)
|
|
{
|
|
if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_ACK)
|
|
{ //__mavlink_waypoint_request_t
|
|
Console.WriteLine("set wp " + index + " ACK 47 : " + buffer[5]);
|
|
break;
|
|
}
|
|
else if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_REQUEST)
|
|
{
|
|
__mavlink_waypoint_request_t ans = new __mavlink_waypoint_request_t();
|
|
|
|
object temp = (object)ans;
|
|
|
|
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
|
|
|
|
ByteArrayToStructure(buffer, ref temp, 6);
|
|
|
|
ans = (__mavlink_waypoint_request_t)(temp);
|
|
|
|
if (ans.seq == (index + 1))
|
|
{
|
|
Console.WriteLine("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]);
|
|
MainV2.givecomport = false;
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine("set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + buffer[5] + " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component);
|
|
//break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// used for last bad serial characters
|
|
/// </summary>
|
|
byte[] lastbad = new byte[2];
|
|
|
|
/// <summary>
|
|
/// Serial Reader to read mavlink packets. POLL method
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public byte[] readPacket()
|
|
{
|
|
byte[] temp = new byte[300];
|
|
int count = 0;
|
|
int length = 0;
|
|
int readcount = 0;
|
|
lastbad = new byte[2];
|
|
|
|
BaseStream.ReadTimeout = 1100; // 1100 ms between bytes
|
|
|
|
DateTime start = DateTime.Now;
|
|
|
|
lock (readlock)
|
|
{
|
|
|
|
while (BaseStream.IsOpen || logreadmode)
|
|
{
|
|
try
|
|
{
|
|
if (readcount > 300)
|
|
{
|
|
Console.WriteLine("MAVLink readpacket No valid mavlink packets");
|
|
break;
|
|
}
|
|
readcount++;
|
|
if (logreadmode)
|
|
{
|
|
try
|
|
{
|
|
if (logplaybackfile.BaseStream.Position == 0)
|
|
{
|
|
if (logplaybackfile.PeekChar() == '-')
|
|
{
|
|
oldlogformat = true;
|
|
}
|
|
else
|
|
{
|
|
oldlogformat = false;
|
|
}
|
|
}
|
|
}
|
|
catch { oldlogformat = false; }
|
|
|
|
if (oldlogformat)
|
|
{
|
|
temp = readlogPacket(); //old style log
|
|
}
|
|
else
|
|
{
|
|
temp = readlogPacketMavlink();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
temp[count] = (byte)BaseStream.ReadByte();
|
|
}
|
|
}
|
|
catch (Exception e) { Console.WriteLine("MAVLink readpacket read error: " + e.Message); break; }
|
|
|
|
if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync
|
|
{
|
|
if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n')
|
|
{
|
|
Console.Write((char)temp[0]);
|
|
}
|
|
count = 0;
|
|
lastbad[0] = lastbad[1];
|
|
lastbad[1] = temp[0];
|
|
temp[1] = 0;
|
|
continue;
|
|
}
|
|
// reset count on valid packet
|
|
readcount = 0;
|
|
|
|
if (temp[0] == 'U' || temp[0] == 254)
|
|
{
|
|
length = temp[1] + 6 + 2 - 2; // data + header + checksum - U - length
|
|
if (count >= 5 || logreadmode)
|
|
{
|
|
if (sysid != 0)
|
|
{
|
|
if (sysid != temp[3] || compid != temp[4])
|
|
{
|
|
Console.WriteLine("Mavlink Bad Packet (not addressed to this MAV) got {0} {1} vs {2} {3}", temp[3], temp[4], sysid, compid);
|
|
return new byte[0];
|
|
}
|
|
}
|
|
|
|
try
|
|
{
|
|
if (logreadmode)
|
|
{
|
|
|
|
}
|
|
else
|
|
{
|
|
int to = 0;
|
|
while (BaseStream.BytesToRead < (length - 4))
|
|
{
|
|
if (to > 1000)
|
|
{
|
|
Console.WriteLine("MAVLINK: wait time out btr {0} len {1}", BaseStream.BytesToRead, length);
|
|
break;
|
|
}
|
|
System.Threading.Thread.Sleep(1);
|
|
System.Windows.Forms.Application.DoEvents();
|
|
to++;
|
|
|
|
//Console.WriteLine("data " + 0 + " " + length + " aval " + BaseStream.BytesToRead);
|
|
}
|
|
int read = BaseStream.Read(temp, 6, length - 4);
|
|
}
|
|
//Console.WriteLine("data " + read + " " + length + " aval " + this.BytesToRead);
|
|
count = length + 2;
|
|
}
|
|
catch { break; }
|
|
break;
|
|
}
|
|
}
|
|
|
|
count++;
|
|
if (count == 299)
|
|
break;
|
|
}
|
|
}// end readlock
|
|
|
|
Array.Resize<byte>(ref temp, count);
|
|
|
|
if (bpstime.Second != DateTime.Now.Second && !logreadmode)
|
|
{
|
|
Console.WriteLine("bps {0} loss {1} left {2}", bps1, synclost, BaseStream.BytesToRead);
|
|
bps2 = bps1; // prev sec
|
|
bps1 = 0; // current sec
|
|
bpstime = DateTime.Now;
|
|
}
|
|
|
|
bps1 += temp.Length;
|
|
|
|
bps = (bps1 + bps2) / 2;
|
|
|
|
if (temp.Length >= 5 && temp[3] == 255 && logreadmode) // gcs packet
|
|
{
|
|
return temp;// new byte[0];
|
|
}
|
|
|
|
ushort crc = crc_calculate(temp, temp.Length - 2);
|
|
|
|
if (temp.Length > 5 && temp[0] == 254)
|
|
{
|
|
crc = crc_accumulate(MAVLINK_MESSAGE_CRCS[temp[5]], crc);
|
|
}
|
|
|
|
if (temp.Length < 5 || temp[temp.Length - 1] != (crc >> 8) || temp[temp.Length - 2] != (crc & 0xff))
|
|
{
|
|
int packetno = 0;
|
|
if (temp.Length > 5)
|
|
{
|
|
packetno = temp[5];
|
|
}
|
|
Console.WriteLine("Mavlink Bad Packet (crc fail) len {0} crc {1} pkno {2}", temp.Length, crc, packetno);
|
|
return new byte[0];
|
|
}
|
|
|
|
try
|
|
{
|
|
if ((temp[0] == 'U' || temp[0] == 254) && temp.Length >= temp[1])
|
|
{
|
|
if (temp[2] != ((recvpacketcount + 1) % 0x100))
|
|
{
|
|
synclost++; // actualy sync loss's
|
|
|
|
if (temp[2] < ((recvpacketcount + 1) % 0x100))
|
|
{
|
|
packetslost += 0x100 - recvpacketcount + temp[2];
|
|
}
|
|
else
|
|
{
|
|
packetslost += temp[2] - recvpacketcount;
|
|
}
|
|
|
|
Console.WriteLine("lost {0} pkts {1}", temp[2], (int)packetslost);
|
|
}
|
|
|
|
if (packetlosttimer.AddSeconds(10) < DateTime.Now)
|
|
{
|
|
packetlosttimer = DateTime.Now;
|
|
packetslost = (int)(packetslost *0.8f);
|
|
packetsnotlost = (int)(packetsnotlost *0.8f);
|
|
}
|
|
|
|
MainV2.cs.linkqualitygcs = (ushort)((packetsnotlost / (packetsnotlost + packetslost)) * 100);
|
|
|
|
packetsnotlost++;
|
|
|
|
recvpacketcount = temp[2];
|
|
|
|
//MAVLINK_MSG_ID_GPS_STATUS
|
|
//if (temp[5] == MAVLINK_MSG_ID_GPS_STATUS)
|
|
|
|
// Console.Write(temp[5] + " " + DateTime.Now.Millisecond + " " + packetspersecond[temp[5]] + " " + (DateTime.Now - packetspersecondbuild[temp[5]]).TotalMilliseconds + " \n");
|
|
|
|
if (double.IsInfinity(packetspersecond[temp[5]]))
|
|
packetspersecond[temp[5]] = 0;
|
|
|
|
packetspersecond[temp[5]] = (((1000 / ((DateTime.Now - packetspersecondbuild[temp[5]]).TotalMilliseconds) + packetspersecond[temp[5]]) / 2));
|
|
|
|
packetspersecondbuild[temp[5]] = DateTime.Now;
|
|
|
|
//Console.WriteLine("Packet {0}",temp[5]);
|
|
// store packet history
|
|
lock (objlock)
|
|
{
|
|
packets[temp[5]] = temp;
|
|
}
|
|
|
|
if (debugmavlink)
|
|
DebugPacket(temp);
|
|
|
|
if (temp[5] == MAVLink.MAVLINK_MSG_ID_STATUSTEXT) // status text
|
|
{
|
|
string logdata = DateTime.Now.Millisecond + " " + Encoding.ASCII.GetString(temp, 6, temp.Length - 6);
|
|
int ind = logdata.IndexOf('\0');
|
|
if (ind != -1)
|
|
logdata = logdata.Substring(0, ind);
|
|
Console.WriteLine(logdata);
|
|
}
|
|
|
|
if (temp[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT)
|
|
{
|
|
// clear old
|
|
wps = new PointLatLngAlt[wps.Length];
|
|
}
|
|
|
|
if (temp[5] == MAVLink.MAVLINK_MSG_ID_WAYPOINT)
|
|
{
|
|
__mavlink_waypoint_t wp = new __mavlink_waypoint_t();
|
|
|
|
object structtemp = (object)wp;
|
|
|
|
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
|
|
|
|
ByteArrayToStructure(temp, ref structtemp, 6);
|
|
|
|
wp = (__mavlink_waypoint_t)(structtemp);
|
|
|
|
wps[wp.seq] = new PointLatLngAlt(wp.x,wp.y,wp.z,wp.seq.ToString());
|
|
}
|
|
|
|
try
|
|
{
|
|
if (logfile != null)
|
|
{
|
|
lock (logwritelock)
|
|
{
|
|
byte[] datearray = BitConverter.GetBytes((UInt64)((DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds * 1000)); //ASCIIEncoding.ASCII.GetBytes(DateTime.Now.ToBinary() + ":");
|
|
Array.Reverse(datearray);
|
|
logfile.Write(datearray, 0, datearray.Length);
|
|
logfile.Write(temp, 0, temp.Length);
|
|
}
|
|
}
|
|
|
|
}
|
|
catch { }
|
|
}
|
|
}
|
|
catch { }
|
|
|
|
lastvalidpacket = DateTime.Now;
|
|
|
|
// Console.Write((DateTime.Now - start).TotalMilliseconds.ToString("00.000") + "\t" + temp.Length + " \r");
|
|
|
|
return temp;
|
|
}
|
|
|
|
byte[] readlogPacket()
|
|
{
|
|
byte[] temp = new byte[300];
|
|
|
|
sysid = 0;
|
|
|
|
int a = 0;
|
|
while (a < temp.Length && logplaybackfile.BaseStream.Position != logplaybackfile.BaseStream.Length)
|
|
{
|
|
temp[a] = (byte)logplaybackfile.BaseStream.ReadByte();
|
|
//Console.Write((char)temp[a]);
|
|
if (temp[a] == ':')
|
|
{
|
|
break;
|
|
}
|
|
a++;
|
|
if (temp[0] != '-')
|
|
{
|
|
a = 0;
|
|
}
|
|
}
|
|
|
|
//Console.Write('\n');
|
|
|
|
//Encoding.ASCII.GetString(temp, 0, a);
|
|
string datestring = Encoding.ASCII.GetString(temp, 0, a);
|
|
//Console.WriteLine(datestring);
|
|
long date = Int64.Parse(datestring);
|
|
DateTime date1 = DateTime.FromBinary(date);
|
|
|
|
lastlogread = date1;
|
|
|
|
int length = 5;
|
|
a = 0;
|
|
while (a < length)
|
|
{
|
|
temp[a] = (byte)logplaybackfile.BaseStream.ReadByte();
|
|
if (a == 1)
|
|
{
|
|
length = temp[1] + 6 + 2 + 1;
|
|
}
|
|
a++;
|
|
}
|
|
|
|
return temp;
|
|
}
|
|
|
|
byte[] readlogPacketMavlink()
|
|
{
|
|
byte[] temp = new byte[300];
|
|
|
|
sysid = 0;
|
|
|
|
//byte[] datearray = BitConverter.GetBytes((ulong)(DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds);
|
|
|
|
byte[] datearray = new byte[8];
|
|
|
|
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);
|
|
|
|
date1 = date1.AddMilliseconds(dateint / 1000);
|
|
|
|
lastlogread = date1;
|
|
|
|
MainV2.cs.datetime = lastlogread;
|
|
|
|
int length = 5;
|
|
int a = 0;
|
|
while (a < length)
|
|
{
|
|
temp[a] = (byte)logplaybackfile.ReadByte();
|
|
if (temp[0] != 'U')
|
|
{
|
|
Console.WriteLine("lost sync byte {0} pos {1}", temp[0], logplaybackfile.BaseStream.Position);
|
|
a = 0;
|
|
continue;
|
|
}
|
|
if (a == 1)
|
|
{
|
|
length = temp[1] + 6 + 2; // 6 header + 2 checksum
|
|
}
|
|
a++;
|
|
}
|
|
|
|
return temp;
|
|
}
|
|
|
|
const int X25_INIT_CRC = 0xffff;
|
|
const int X25_VALIDATE_CRC = 0xf0b8;
|
|
|
|
ushort crc_accumulate(byte b, ushort crc)
|
|
{
|
|
unchecked
|
|
{
|
|
byte ch = (byte)(b ^ (byte)(crc & 0x00ff));
|
|
ch = (byte)(ch ^ (ch << 4));
|
|
return (ushort)((crc >> 8) ^ (ch << 8) ^ (ch << 3) ^ (ch >> 4));
|
|
}
|
|
}
|
|
|
|
ushort crc_calculate(byte[] pBuffer, int length)
|
|
{
|
|
if (length < 1)
|
|
{
|
|
return 0xffff;
|
|
}
|
|
// For a "message" of length bytes contained in the unsigned char array
|
|
// pointed to by pBuffer, calculate the CRC
|
|
// crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed
|
|
|
|
ushort crcTmp;
|
|
int i;
|
|
|
|
crcTmp = X25_INIT_CRC;
|
|
|
|
for (i = 1; i < length; i++) // skips header U
|
|
{
|
|
crcTmp = crc_accumulate(pBuffer[i], crcTmp);
|
|
//Console.WriteLine(crcTmp + " " + pBuffer[i] + " " + length);
|
|
}
|
|
|
|
return (crcTmp);
|
|
}
|
|
|
|
|
|
public static byte[] StructureToByteArray(object obj)
|
|
{
|
|
|
|
int len = Marshal.SizeOf(obj);
|
|
|
|
byte[] arr = new byte[len];
|
|
|
|
IntPtr ptr = Marshal.AllocHGlobal(len);
|
|
|
|
Marshal.StructureToPtr(obj, ptr, true);
|
|
|
|
Marshal.Copy(ptr, arr, 0, len);
|
|
|
|
Marshal.FreeHGlobal(ptr);
|
|
|
|
return arr;
|
|
|
|
}
|
|
|
|
public static void ByteArrayToStructure(byte[] bytearray, ref object obj, int startoffset)
|
|
{
|
|
if (bytearray[0] == 'U')
|
|
{
|
|
ByteArrayToStructureEndian(bytearray, ref obj, startoffset);
|
|
}
|
|
else
|
|
{
|
|
int len = Marshal.SizeOf(obj);
|
|
|
|
IntPtr i = Marshal.AllocHGlobal(len);
|
|
|
|
// create structure from ptr
|
|
obj = Marshal.PtrToStructure(i, obj.GetType());
|
|
|
|
try
|
|
{
|
|
// copy byte array to ptr
|
|
Marshal.Copy(bytearray, startoffset, i, len);
|
|
}
|
|
catch (Exception ex) { Console.WriteLine("ByteArrayToStructure FAIL: error " + ex.ToString()); }
|
|
|
|
obj = Marshal.PtrToStructure(i, obj.GetType());
|
|
|
|
Marshal.FreeHGlobal(i);
|
|
}
|
|
}
|
|
|
|
public static void ByteArrayToStructureEndian(byte[] bytearray, ref object obj, int startoffset)
|
|
{
|
|
int len = Marshal.SizeOf(obj);
|
|
|
|
IntPtr i = Marshal.AllocHGlobal(len);
|
|
|
|
byte[] temparray = (byte[])bytearray.Clone();
|
|
|
|
// create structure from ptr
|
|
obj = Marshal.PtrToStructure(i, obj.GetType());
|
|
|
|
// do endian swap
|
|
|
|
object thisBoxed = obj;
|
|
Type test = thisBoxed.GetType();
|
|
|
|
int reversestartoffset = startoffset;
|
|
|
|
// Enumerate each structure field using reflection.
|
|
foreach (var field in test.GetFields())
|
|
{
|
|
// field.Name has the field's name.
|
|
|
|
object fieldValue = field.GetValue(thisBoxed); // Get value
|
|
|
|
// Get the TypeCode enumeration. Multiple types get mapped to a common typecode.
|
|
TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType());
|
|
|
|
if (typeCode != TypeCode.Object)
|
|
{
|
|
Array.Reverse(temparray, reversestartoffset, Marshal.SizeOf(fieldValue));
|
|
reversestartoffset += Marshal.SizeOf(fieldValue);
|
|
}
|
|
else
|
|
{
|
|
reversestartoffset += ((byte[])fieldValue).Length;
|
|
}
|
|
|
|
}
|
|
|
|
try
|
|
{
|
|
// copy byte array to ptr
|
|
Marshal.Copy(temparray, startoffset, i, len);
|
|
}
|
|
catch (Exception ex) { Console.WriteLine("ByteArrayToStructure FAIL: error " + ex.ToString()); }
|
|
|
|
obj = Marshal.PtrToStructure(i, obj.GetType());
|
|
|
|
Marshal.FreeHGlobal(i);
|
|
|
|
}
|
|
|
|
public short swapend11(short value)
|
|
{
|
|
int len = Marshal.SizeOf(value);
|
|
|
|
byte[] temp = BitConverter.GetBytes(value);
|
|
|
|
Array.Reverse(temp);
|
|
|
|
return BitConverter.ToInt16(temp, 0);
|
|
}
|
|
|
|
public ushort swapend11(ushort value)
|
|
{
|
|
int len = Marshal.SizeOf(value);
|
|
|
|
byte[] temp = BitConverter.GetBytes(value);
|
|
|
|
Array.Reverse(temp);
|
|
|
|
return BitConverter.ToUInt16(temp, 0);
|
|
}
|
|
|
|
public ulong swapend11(ulong value)
|
|
{
|
|
int len = Marshal.SizeOf(value);
|
|
|
|
byte[] temp = BitConverter.GetBytes(value);
|
|
|
|
Array.Reverse(temp);
|
|
|
|
return BitConverter.ToUInt64(temp, 0);
|
|
}
|
|
|
|
public float swapend11(float value)
|
|
{
|
|
byte[] temp = BitConverter.GetBytes(value);
|
|
if (temp[0] == 0xff)
|
|
temp[0] = 0xfe;
|
|
Array.Reverse(temp);
|
|
return BitConverter.ToSingle(temp, 0);
|
|
}
|
|
|
|
public int swapend11(int value)
|
|
{
|
|
int len = Marshal.SizeOf(value);
|
|
|
|
byte[] temp = BitConverter.GetBytes(value);
|
|
|
|
Array.Reverse(temp);
|
|
|
|
return BitConverter.ToInt32(temp, 0);
|
|
}
|
|
|
|
public double swapend11(double value)
|
|
{
|
|
int len = Marshal.SizeOf(value);
|
|
|
|
byte[] temp = BitConverter.GetBytes(value);
|
|
|
|
Array.Reverse(temp);
|
|
|
|
return BitConverter.ToDouble(temp, 0);
|
|
}
|
|
}
|
|
} |