public MAV_MISSION_RESULT setWP(mavlink_mission_item_int_t req) { giveComport = true; ushort index = req.seq; log.InfoFormat("setWPint {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x / 1.0e7, req.y /1.0e7 , req.z, index); // request generatePacket((byte)MAVLINK_MSG_ID.MISSION_ITEM_INT, req); DateTime start = DateTime.Now; int retrys = 10; while (true) { if (!(start.AddMilliseconds(400) > DateTime.Now)) { if (retrys > 0) { log.Info("setWP Retry " + retrys); generatePacket((byte)MAVLINK_MSG_ID.MISSION_ITEM_INT, req); start = DateTime.Now; retrys--; continue; } giveComport = false; throw new TimeoutException("Timeout on read - setWP"); } MAVLinkMessage buffer = readPacket(); if (buffer.Length > 5) { if (buffer.msgid == (byte)MAVLINK_MSG_ID.MISSION_ACK) { var ans = buffer.ToStructure<mavlink_mission_ack_t>(); log.Info("set wp " + index + " ACK 47 : " + buffer.msgid + " ans " + Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString())); giveComport = false; if (req.current == 2) { MAV.GuidedMode = (Locationwp)req; } else if (req.current == 3) { } else { MAV.wps[req.seq] = (Locationwp)req; } return (MAV_MISSION_RESULT)ans.type; } else if (buffer.msgid == (byte)MAVLINK_MSG_ID.MISSION_REQUEST) { var ans = buffer.ToStructure<mavlink_mission_request_t>(); if (ans.seq == (index + 1)) { log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer.msgid); giveComport = false; if (req.current == 2) { MAV.GuidedMode = (Locationwp)req; } else if (req.current == 3) { } else { MAV.wps[req.seq] = (Locationwp)req; } return MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED; } else { log.InfoFormat( "set wp fail doing " + index + " req " + ans.seq + " ACK 47 or REQ 40 : " + buffer.msgid + " seq {0} ts {1} tc {2}", req.seq, req.target_system, req.target_component); // resend point now start = DateTime.MinValue; } } else { //Console.WriteLine(DateTime.Now + " PC setwp " + buffer.msgid); } } } // return MAV_MISSION_RESULT.MAV_MISSION_INVALID; }
/// <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 MAV_MISSION_RESULT setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current = 0, byte autocontinue = 1, bool use_int = false) { if (use_int) { mavlink_mission_item_int_t req = new mavlink_mission_item_int_t(); req.target_system = MAV.sysid; req.target_component = MAV.compid; req.command = loc.id; req.current = current; req.autocontinue = autocontinue; req.frame = (byte) frame; req.y = (int) (loc.lng*1.0e7); req.x = (int) (loc.lat*1.0e7); req.z = (float) (loc.alt); req.param1 = loc.p1; req.param2 = loc.p2; req.param3 = loc.p3; req.param4 = loc.p4; req.seq = index; return setWP(req); } else { mavlink_mission_item_t req = new mavlink_mission_item_t(); req.target_system = MAV.sysid; req.target_component = MAV.compid; req.command = loc.id; req.current = current; req.autocontinue = autocontinue; 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; req.seq = index; return setWP(req); } }