private double GetHomeAlt(MAVLink.MAV_FRAME altmode, double homealt, double lat, double lng) { if (altmode == MAVLink.MAV_FRAME.GLOBAL_INT || altmode == MAVLink.MAV_FRAME.GLOBAL) { return(0); // for absolute we dont need to add homealt } if (altmode == MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT_INT || altmode == MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT) { return(srtm.getAltitude(lat, lng).alt); } return(homealt); }
private double GetHomeAlt(MAVLink.MAV_FRAME altmode, double homealt, double lat, double lng) { if (altmode == MAVLink.MAV_FRAME.GLOBAL_INT || altmode == MAVLink.MAV_FRAME.GLOBAL) { return(0); // for absolute we dont need to add homealt } if (altmode == MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT_INT || altmode == MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT) { var sralt = srtm.getAltitude(lat, lng); if (sralt.currenttype == srtm.tiletype.invalid) { return(-999); } return(sralt.alt); } return(homealt); }
private MAVLink.MAV_MISSION_RESULT setWP(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0, byte autocontinue = 1) { MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t(); req.target_system = TARGET_SYSTEM_ID; req.target_component = TARGET_SYS_COMPID; // MSG_NAMES.MISSION_ITEM 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)); }
public void CreateOverlay(MAVLink.MAV_FRAME altmode, PointLatLngAlt home, List <Locationwp> missionitems, double wpradius, double loiterradius) { overlay.Clear(); double maxlat = -180; double maxlong = -180; double minlat = 180; double minlong = 180; Func <double, double, double> gethomealt = (lat, lng) => GetHomeAlt(altmode, home.Alt, lat, lng); home.Tag = "H"; home.Tag2 = altmode.ToString(); pointlist.Add(home); fullpointlist.Add(pointlist[pointlist.Count - 1]); addpolygonmarker("H", home.Lng, home.Lat, home.Alt, null, 0); int a = 0; foreach (var itemtuple in missionitems.PrevNowNext()) { var itemprev = itemtuple.Item1; var item = itemtuple.Item2; var itemnext = itemtuple.Item3; ushort command = item.id; // invalid locationwp if (command == 0) { continue; } if (command < (ushort)MAVLink.MAV_CMD.LAST && command != (ushort)MAVLink.MAV_CMD.TAKEOFF && // doesnt have a position command != (ushort)MAVLink.MAV_CMD.VTOL_TAKEOFF && // doesnt have a position command != (ushort)MAVLink.MAV_CMD.RETURN_TO_LAUNCH && command != (ushort)MAVLink.MAV_CMD.CONTINUE_AND_CHANGE_ALT && command != (ushort)MAVLink.MAV_CMD.DELAY && command != (ushort)MAVLink.MAV_CMD.GUIDED_ENABLE || command == (ushort)MAVLink.MAV_CMD.DO_SET_ROI) { // land can be 0,0 or a lat,lng if (command == (ushort)MAVLink.MAV_CMD.LAND && item.lat == 0 && item.lng == 0) { continue; } if (command == (ushort)MAVLink.MAV_CMD.DO_SET_ROI) { pointlist.Add(new PointLatLngAlt(item.lat, item.lng, item.alt + gethomealt(item.lat, item.lng), "ROI" + (a + 1)) { color = Color.Red }); // do set roi is not a nav command. so we dont route through it //fullpointlist.Add(pointlist[pointlist.Count - 1]); GMarkerGoogle m = new GMarkerGoogle(new PointLatLng(item.lat, item.lng), GMarkerGoogleType.red); m.ToolTipMode = MarkerTooltipMode.Always; m.ToolTipText = (a + 1).ToString(); m.Tag = (a + 1).ToString(); GMapMarkerRect mBorders = new GMapMarkerRect(m.Position); { mBorders.InnerMarker = m; mBorders.Tag = "Dont draw line"; } // check for clear roi, and hide it if (m.Position.Lat != 0 && m.Position.Lng != 0) { // order matters overlay.Markers.Add(m); overlay.Markers.Add(mBorders); } } else if (command == (ushort)MAVLink.MAV_CMD.LOITER_TIME || command == (ushort)MAVLink.MAV_CMD.LOITER_TURNS || command == (ushort)MAVLink.MAV_CMD.LOITER_UNLIM) { pointlist.Add(new PointLatLngAlt(item.lat, item.lng, item.alt + gethomealt(item.lat, item.lng), (a + 1).ToString()) { color = Color.LightBlue }); // exit at tangent if (item.p4 == 1) { var from = pointlist.Last(); var to = itemnext.lat != 0 && itemnext.lng != 0 ? new PointLatLngAlt(itemnext) { Alt = itemnext.alt + gethomealt(item.lat, item.lng) } : from; var bearing = from.GetBearing(to); var dist = from.GetDistance(to); if (dist > loiterradius) { fullpointlist.Add(pointlist[pointlist.Count - 1]); var offset = from.newpos(bearing + 90, loiterradius); fullpointlist.Add(offset); } else { fullpointlist.Add(pointlist[pointlist.Count - 1]); } } else { fullpointlist.Add(pointlist[pointlist.Count - 1]); } addpolygonmarker((a + 1).ToString(), item.lng, item.lat, item.alt, Color.LightBlue, loiterradius); } else if (command == (ushort)MAVLink.MAV_CMD.SPLINE_WAYPOINT) { pointlist.Add(new PointLatLngAlt(item.lat, item.lng, item.alt + gethomealt(item.lat, item.lng), (a + 1).ToString()) { Tag2 = "spline" }); fullpointlist.Add(pointlist[pointlist.Count - 1]); addpolygonmarker((a + 1).ToString(), item.lng, item.lat, item.alt, Color.Green, wpradius); } else { pointlist.Add(new PointLatLngAlt(item.lat, item.lng, item.alt + gethomealt(item.lat, item.lng), (a + 1).ToString())); fullpointlist.Add(pointlist[pointlist.Count - 1]); addpolygonmarker((a + 1).ToString(), item.lng, item.lat, item.alt, null, wpradius); } maxlong = Math.Max(item.lng, maxlong); maxlat = Math.Max(item.lat, maxlat); minlong = Math.Min(item.lng, minlong); minlat = Math.Min(item.lat, minlat); } else if (command == (ushort)MAVLink.MAV_CMD.DO_JUMP) // fix do jumps into the future { pointlist.Add(null); int wpno = (int)item.p1; int repeat = (int)item.p2; List <PointLatLngAlt> list = new List <PointLatLngAlt>(); // cycle through reps for (int repno = repeat; repno > 0; repno--) { // cycle through wps for (int no = wpno; no <= a; no++) { if (pointlist[no] != null) { list.Add(pointlist[no]); } } } fullpointlist.AddRange(list); } else { pointlist.Add(null); } a++; } RegenerateWPRoute(fullpointlist, home); }
public static void upload(MAVLinkInterface port, MAVLink.MAV_MISSION_TYPE type, List <Locationwp> commandlist, MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT) { try { if (!port.BaseStream.IsOpen) { throw new Exception("Please connect first!"); } int a; bool use_int = (port.MAV.cs.capabilities & (uint)MAVLink.MAV_PROTOCOL_CAPABILITY.MISSION_INT) > 0; port.setWPTotal((ushort)commandlist.Count); // process commandlist to the mav for (a = 0; a < commandlist.Count; a++) { var temp = commandlist[a]; // handle current wp upload number int uploadwpno = a; // try send the wp MAVLink.MAV_MISSION_RESULT ans = port.setWP(temp, (ushort)(uploadwpno), (MAVLink.MAV_FRAME)temp.frame, 0, 1, use_int, type); // we timed out while uploading wps/ command wasnt replaced/ command wasnt added if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ERROR) { // resend for partial upload port.setWPPartialUpdate((ushort)(uploadwpno), (ushort)commandlist.Count, type); // reupload this point. ans = port.setWP(temp, (ushort)(uploadwpno), frame, 0, 1, use_int, type); } if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_NO_SPACE) { throw new Exception("Upload failed, please reduce the number of wp's"); } if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID) { throw new Exception( "Upload failed, mission was rejected by the Mav,\n item had a bad option wp# " + a + " " + ans); } if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID_SEQUENCE) { // invalid sequence can only occur if we failed to see a response from the apm when we sent the request. // or there is io lag and we send 2 mission_items and get 2 responces, one valid, one a ack of the second send // the ans is received via mission_ack, so we dont know for certain what our current request is for. as we may have lost the mission_request // get requested wp no - 1; try { a = port.getRequestedWPNo() - 1; } catch { // resend for partial upload port.setWPPartialUpdate((ushort)(uploadwpno), (ushort)commandlist.Count, type); // reupload this point. } continue; } if (ans != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED) { throw new Exception("Upload wps failed " + Enum.Parse(typeof(MAVLink.MAV_CMD), temp.id.ToString()) + " " + Enum.Parse(typeof(MAVLink.MAV_MISSION_RESULT), ans.ToString())); } } port.setWPACK(); } catch (Exception ex) { log.Error(ex); throw; } }
public void sendWPS(List <PointLatLngAlt> Points) { MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT; Locationwp home = new Locationwp(); try { home.id = (byte)MAVLink.MAV_CMD.WAYPOINT; home.lat = 23.1647061918178; home.lng = 113.455638885498; home.alt = 20; // use saved home } catch { throw new Exception("Your home location is invalid"); } ushort count = (ushort)(Points.Count + 1);//含家地址 UpLoadForm upform = new UpLoadForm(); upform.StartPosition = FormStartPosition.Manual; upform.Location = new System.Drawing.Point((this.form.Width - upform.Width) / 2, (this.form.Height - upform.Height) / 2); // this.upform.Visible = true; // Application.DoEvents(); upform.Show(); // Application.DoEvents(); upform.SetMessage(0, count); // this.upform.ShowDialog(); // Thread.Sleep(100); setWPTotal(count); this.ListMessage("已发送航点数量..."); upform.SetMessage(1, 0); bool mission_uploading = true; Locationwp[] wp_location = new Locationwp[count - 1]; int i = 0; foreach (PointLatLngAlt point in Points) { wp_location[i].id = (byte)MAVLink.MAV_CMD.WAYPOINT; wp_location[i].lat = point.Lat; wp_location[i].lng = point.Lng; wp_location[i].alt = point.Alt; i++; } int seq = 0; bool isSendTotalSuccess = false; int cnt = 0; while (mission_uploading) { Program.mav_msg_handler.mission_request = false; Program.mav_msg_handler.mission_ack = false; Program.mav_msg_handler.SetTimeout(5); while (true) { if (Program.mav_msg_handler.Wait()) { if (isSendTotalSuccess) { ListMessage("发送航点超时出错"); mission_uploading = false; break; } else { cnt++; if (cnt >= 3) { ListMessage("发送航点数量超时出错"); mission_uploading = false; break; } else { ListMessage("发送航点数量超时出错,准备重发..."); setWPTotal(count); break; } } } if (upform.isCancel) { this.ListMessage("您已经取消航点发送..."); upform.Close(); return; } if (Program.mav_msg_handler.mission_ack) { // this.ListMessage("收到ACK..."); if (Program.mav_msg_handler.mavlink_mission_ack.type != 0) { ListMessage("发送航点出错"); } else { ListMessage("航点上传完毕"); } mission_uploading = false; break; } if (Program.mav_msg_handler.mission_request == true) { isSendTotalSuccess = true; // this.ListMessage("收到REQUEST..."); seq = Program.mav_msg_handler.mavlink_mission_request.seq; if (seq == 0) { setWP(home, 0, MAVLink.MAV_FRAME.GLOBAL, 0); ListMessage("正在发送HOME..."); } else { setWP(wp_location[seq - 1], (ushort)(seq), frame, 0); ListMessage("正在发送航点" + seq.ToString() + "..."); upform.SetMessage(2, seq); upform.SetCount(seq, count); } break; } Application.DoEvents(); } } Thread.Sleep(500); upform.SetMessage(3, 0); Thread.Sleep(500); setWPACK(); upform.Close(); }