public void setNewWPAlt(Locationwp gotohere) { giveComport = true; try { gotohere.id = (byte)MAV_CMD.WAYPOINT; MAV_MISSION_RESULT ans = setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)3); if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED) throw new Exception("Alt Change Failed"); } catch (Exception ex) { giveComport = false; //log.Error(ex); throw; } giveComport = false; }
public void setGuidedModeWP(Locationwp gotohere) { if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0) return; giveComport = true; try { gotohere.id = (byte)MAV_CMD.WAYPOINT; MAV_MISSION_RESULT ans = setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2); if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED) throw new Exception("Guided Mode Failed"); } catch (Exception ex) { } giveComport = false; }
/// <summary> /// Gets specfied WP /// </summary> /// <param name="index"></param> /// <returns>WP</returns> public Locationwp getWP(ushort index) { giveComport = true; Locationwp loc = new Locationwp(); 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) { //log.Info("getWP Retry " + retrys); generatePacket(MAVLINK_MSG_ID_MISSION_REQUEST, req); start = DateTime.Now; retrys--; continue; } 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); //Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6); var wp = buffer.ByteArrayToStructure<mavlink_mission_item_t>(6); 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 (MAV.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; } } */ //log.InfoFormat("getWP {0} {1} {2} {3} {4} opt {5}", loc.id, loc.p1, loc.alt, loc.lat, loc.lng, loc.options); break; } else { //log.Info(DateTime.Now + " PC getwp " + buffer[5]); } } } giveComport = false; return loc; }
/// <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) { giveComport = true; mavlink_mission_item_t req = new mavlink_mission_item_t(); 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; req.seq = index; //log.InfoFormat("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 generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req); DateTime start = DateTime.Now; int retrys = 10; while (true) { if (!(start.AddMilliseconds(150) > DateTime.Now)) { if (retrys > 0) { //log.Info("setWP Retry " + retrys); generatePacket(MAVLINK_MSG_ID_MISSION_ITEM, req); start = DateTime.Now; retrys--; continue; } giveComport = false; throw new Exception("Timeout on read - setWP"); } byte[] buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_MISSION_ACK) { var ans = buffer.ByteArrayToStructure<mavlink_mission_ack_t>(6); //log.Info("set wp " + index + " ACK 47 : " + buffer[5] + " ans " + Enum.Parse(typeof(MAV_MISSION_RESULT), ans.type.ToString())); if (req.current == 2) { GuidedMode = req; } else if (req.current == 3) { } else { wps[req.seq] = req; } return (MAV_MISSION_RESULT)ans.type; } else if (buffer[5] == MAVLINK_MSG_ID_MISSION_REQUEST) { var ans = buffer.ByteArrayToStructure<mavlink_mission_request_t>(6); if (ans.seq == (index + 1)) { //log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]); giveComport = false; if (req.current == 2) { GuidedMode = req; } else if (req.current == 3) { } else { wps[req.seq] = 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[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]); } } } // return MAV_MISSION_RESULT.MAV_MISSION_INVALID; }
public PointLatLngAlt(Locationwp locwp) { this.Lat = locwp.lat; this.Lng = locwp.lng; this.Alt = locwp.alt; }