/// <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;
#if MAVLINK10
__mavlink_mission_item_t req = new __mavlink_mission_item_t();
#else
__mavlink_waypoint_t req = new __mavlink_waypoint_t();
#endif
req.target_system = sysid;
req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM
req.command = loc.id;
req.param1 = loc.p1;
req.current = current;
req.frame = (byte)frame;
req.y = (float)(loc.lng);
req.x = (float)(loc.lat);
req.z = (float)(loc.alt);
req.param1 = loc.p1;
req.param2 = loc.p2;
req.param3 = loc.p3;
req.param4 = loc.p4;
/*
if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane)
{
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.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
#if MAVLINK10
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
#else
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
#endif
DateTime start = DateTime.Now;
int retrys = 6;
while (true)
{
if (!(start.AddMilliseconds(500) > DateTime.Now))
{
if (retrys > 0)
{
Console.WriteLine("setWP Retry " + retrys);
#if MAVLINK10
generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req);
#else
generatePacket(MAVLINK_MSG_ID_WAYPOINT, req);
#endif
start = DateTime.Now;
retrys--;
continue;
}
MainV2.givecomport = false;
throw new Exception("Timeout on read - setWP");
}
byte[] buffer = readPacket();
if (buffer.Length > 5)
{
#if MAVLINK10
if (buffer[5] == MAVLINK_MSG_ID_MISSION_ACK)
{
__mavlink_mission_ack_t ans = new __mavlink_mission_ack_t();
object temp = (object)ans;
ByteArrayToStructure(buffer, ref temp, 6);
ans = (__mavlink_mission_ack_t)(temp);
Console.WriteLine("set wp " + index + " ACK 47 : " + buffer[5] + " ans " + Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString()));
break;
}
else if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST)
{
__mavlink_mission_request_t ans = new __mavlink_mission_request_t();
object temp = (object)ans;
ByteArrayToStructure(buffer, ref temp, 6);
ans = (__mavlink_mission_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]);
}
#else
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]);
}
#endif
}
}
}
/// <summary>
/// Used to extract mission from log file
/// </summary>
/// <param name="temp">packet</param>
void getWPsfromstream(ref byte[] temp )
{
#if MAVLINK10
if (temp[5] == MAVLINK_MSG_ID_MISSION_COUNT)
{
// clear old
wps = new PointLatLngAlt[wps.Length];
}
if (temp[5] == MAVLink.MAVLINK_MSG_ID_MISSION_ITEM)
{
__mavlink_mission_item_t wp = new __mavlink_mission_item_t();
object structtemp = (object)wp;
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
ByteArrayToStructure(temp, ref structtemp, 6);
wp = (__mavlink_mission_item_t)(structtemp);
#else
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);
#endif
wps[wp.seq] = new PointLatLngAlt(wp.x, wp.y, wp.z, wp.seq.ToString());
}
}
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' && temp[0] != 254)
{
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);
}
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;
}
/// <summary>
/// Gets specfied WP
/// </summary>
/// <param name="index"></param>
/// <returns>WP</returns>
public Locationwp getWP(ushort index)
{
MainV2.givecomport = true;
Locationwp loc = new Locationwp();
#if MAVLINK10
__mavlink_mission_request_t req = new __mavlink_mission_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_MISSION_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_MISSION_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_MISSION_ITEM)
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
__mavlink_mission_item_t wp = new __mavlink_mission_item_t();
object temp = (object)wp;
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
ByteArrayToStructure(buffer, ref temp, 6);
wp = (__mavlink_mission_item_t)(temp);
#else
__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);
#endif
loc.options = (byte)(wp.frame & 0x1);
loc.id = (byte)(wp.command);
loc.p1 = (wp.param1);
loc.p2 = (wp.param2);
loc.p3 = (wp.param3);
loc.p4 = (wp.param4);
loc.alt = ((wp.z));
loc.lat = ((wp.x));
loc.lng = ((wp.y));
/*
if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane)
{
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 = (float)((wp.z));
loc.lat = (float)((wp.x));
loc.lng = (float)((wp.y));
loc.p1 = (float)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);
}
