/// <summary> /// Sets wp total count /// </summary> /// <param name="wp_total"></param> public void setWPTotal(ushort wp_total) { giveComport = true; mavlink_mission_count_t req = new mavlink_mission_count_t(); req.target_system = MAV.sysid; req.target_component = MAV.compid; // MSG_NAMES.MISSION_COUNT req.count = wp_total; generatePacket((byte) MAVLINK_MSG_ID.MISSION_COUNT, req); DateTime start = DateTime.Now; int retrys = 3; while (true) { if (!(start.AddMilliseconds(700) > DateTime.Now)) { if (retrys > 0) { log.Info("setWPTotal Retry " + retrys); generatePacket((byte) MAVLINK_MSG_ID.MISSION_COUNT, req); start = DateTime.Now; retrys--; continue; } giveComport = false; throw new Exception("Timeout on read - setWPTotal"); } MAVLinkMessage buffer = readPacket(); if (buffer.Length > 9) { if (buffer.msgid == (byte) MAVLINK_MSG_ID.MISSION_REQUEST) { var request = buffer.ToStructure<mavlink_mission_request_t>(); if (request.seq == 0) { if (MAV.param["WP_TOTAL"] != null) MAV.param["WP_TOTAL"].Value = wp_total - 1; if (MAV.param["CMD_TOTAL"] != null) MAV.param["CMD_TOTAL"].Value = wp_total - 1; if (MAV.param["MIS_TOTAL"] != null) MAV.param["MIS_TOTAL"].Value = wp_total - 1; MAV.wps.Clear(); giveComport = false; return; } } else { //Console.WriteLine(DateTime.Now + " PC getwp " + buffer.msgid); } } } }
/// <summary> /// Sets wp total count /// </summary> /// <param name="wp_total"></param> public void setWPTotal(ushort wp_total) { 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) { log.Info("setWPTotal Retry " + retrys); generatePacket(MAVLINK_MSG_ID_MISSION_COUNT, req); start = DateTime.Now; retrys--; continue; } giveComport = false; throw new Exception("Timeout on read - setWPTotal"); } byte[] buffer = readPacket(); if (buffer.Length > 9) { if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST) { var request = buffer.ByteArrayToStructure<mavlink_mission_request_t>(6); 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; wps.Clear(); giveComport = false; return; } } else { //Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); } } } }