/// <summary> /// Sets wp total count /// </summary> /// <param name="wp_total"></param> public void setWPTotal(ushort wp_total) { #if MAVLINK10 MainV2.givecomport = true; __mavlink_mission_count_t req = new __mavlink_mission_count_t(); req.target_system = sysid; req.target_component = compid; // MAVLINK_MSG_ID_MISSION_COUNT req.count = wp_total; generatePacket(MAVLINK_MSG_ID_MISSION_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_MISSION_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_MISSION_REQUEST) { __mavlink_mission_request_t request = new __mavlink_mission_request_t(); object temp = (object)request; ByteArrayToStructure(buffer, ref temp, 6); request = (__mavlink_mission_request_t)(temp); if (request.seq == 0) { if (param["WP_TOTAL"] != null) param["WP_TOTAL"] = (float)wp_total - 1; if (param["CMD_TOTAL"] != null) param["CMD_TOTAL"] = (float)wp_total - 1; MainV2.givecomport = false; return; } } else { //Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); } } } #else 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) { __mavlink_waypoint_request_t request = new __mavlink_waypoint_request_t(); object temp = (object)request; ByteArrayToStructure(buffer, ref temp, 6); request = (__mavlink_waypoint_request_t)(temp); if (request.seq == 0) { if (param["WP_TOTAL"] != null) param["WP_TOTAL"] = (float)wp_total - 1; if (param["CMD_TOTAL"] != null) param["CMD_TOTAL"] = (float)wp_total - 1; MainV2.givecomport = false; return; } } else { //Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); } } } #endif }
/// <summary> /// Returns WP count /// </summary> /// <returns></returns> public byte getWPCount() { MainV2.givecomport = true; byte[] buffer; #if MAVLINK10 __mavlink_mission_request_list_t req = new __mavlink_mission_request_list_t(); req.target_system = sysid; req.target_component = compid; // request list generatePacket(MAVLINK_MSG_ID_MISSION_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_MISSION_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_MISSION_COUNT) { __mavlink_mission_count_t count = new __mavlink_mission_count_t(); object temp = (object)count; ByteArrayToStructure(buffer, ref temp, 6); count = (__mavlink_mission_count_t)(temp); Console.WriteLine("wpcount: " + count.count); MainV2.givecomport = false; return (byte)count.count; // should be ushort, but apm has limited wp count < byte } else { Console.WriteLine(DateTime.Now + " PC wpcount " + buffer[5] + " need " + MAVLINK_MSG_ID_MISSION_COUNT + " " + this.BaseStream.BytesToRead); } } } #else __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); } } } #endif }