/// <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); }