diff --git a/Tools/ArdupilotMegaPlanner/Common.cs b/Tools/ArdupilotMegaPlanner/Common.cs index 2d8a815197..05a0fdaabf 100644 --- a/Tools/ArdupilotMegaPlanner/Common.cs +++ b/Tools/ArdupilotMegaPlanner/Common.cs @@ -299,21 +299,6 @@ namespace ArdupilotMega OF_LOITER = 10 } - int fixme; - public enum bitmask - { - None = 0, - sonar_enable = 1, - compass_enabled = 2, - optflow_enabled = 4, - super_simple = 8, - waypoint_mode = 16, - esc_calibrate = 32, - heli_ext_gyro_enabled = 64, - heli_servo_averaging = 128, - heli_servo_manual = 256, - } - public static void linearRegression() { double[] values = { 4.8, 4.8, 4.5, 3.9, 4.4, 3.6, 3.6, 2.9, 3.5, 3.0, 2.5, 2.2, 2.6, 2.1, 2.2 }; diff --git a/Tools/ArdupilotMegaPlanner/Controls/AGauge.cs b/Tools/ArdupilotMegaPlanner/Controls/AGauge.cs index 2d2b326f6a..0c405be46c 100644 --- a/Tools/ArdupilotMegaPlanner/Controls/AGauge.cs +++ b/Tools/ArdupilotMegaPlanner/Controls/AGauge.cs @@ -221,13 +221,13 @@ namespace AGaugeApp #region properties [System.ComponentModel.Browsable(true)] - public Single Value0 { get { return m_value[0]; } set { m_NeedIdx = 0; Value = value; this.Invalidate(); } } + public Single Value0 { get { return m_value[0]; } set { m_NeedIdx = 0; m_value[0] = value; this.Invalidate(); } } [System.ComponentModel.Browsable(true)] - public Single Value1 { get { return m_value[1]; } set { m_NeedIdx = 1; Value = value; this.Invalidate(); } } + public Single Value1 { get { return m_value[1]; } set { m_NeedIdx = 1; m_value[1] = value; this.Invalidate(); } } [System.ComponentModel.Browsable(true)] - public Single Value2 { get { return m_value[2]; } set { m_NeedIdx = 2; Value = value; this.Invalidate(); } } + public Single Value2 { get { return m_value[2]; } set { m_NeedIdx = 2; m_value[2] = value; this.Invalidate(); } } [System.ComponentModel.Browsable(true)] - public Single Value3 { get { return m_value[3]; } set { m_NeedIdx = 3; Value = value; this.Invalidate(); } } + public Single Value3 { get { return m_value[3]; } set { m_NeedIdx = 3; m_value[3] = value; this.Invalidate(); } } [System.ComponentModel.Browsable(true), System.ComponentModel.Category("AGauge"), diff --git a/Tools/ArdupilotMegaPlanner/Controls/MyLabel.cs b/Tools/ArdupilotMegaPlanner/Controls/MyLabel.cs index bcd2271dc5..cc18c0b723 100644 --- a/Tools/ArdupilotMegaPlanner/Controls/MyLabel.cs +++ b/Tools/ArdupilotMegaPlanner/Controls/MyLabel.cs @@ -24,7 +24,7 @@ namespace ArdupilotMega public MyLabel() { } - + public override string Text { get @@ -33,6 +33,7 @@ namespace ArdupilotMega } set { + if (value == null) return; @@ -46,8 +47,8 @@ namespace ArdupilotMega noofchars = label.Length; Size textSize = TextRenderer.MeasureText(value, this.Font); this.Width = textSize.Width; - } - + } + this.Invalidate(); } } diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index 3f7fe12c44..57709156c8 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -186,6 +186,59 @@ namespace ArdupilotMega public ushort rcoverridech7 { get; set; } public ushort rcoverridech8 { get; set; } + internal PointLatLngAlt HomeLocation = new PointLatLngAlt(); + + public float DistToMAV + { + get + { + // shrinking factor for longitude going to poles direction + double rads = Math.Abs(HomeLocation.Lat) * 0.0174532925; + double scaleLongDown = Math.Cos(rads); + double scaleLongUp = 1.0f / Math.Cos(rads); + + //DST to Home + double dstlat = Math.Abs(HomeLocation.Lat - lat) * 111319.5; + double dstlon = Math.Abs(HomeLocation.Lng - lng) * 111319.5 * scaleLongDown; + return (float)Math.Sqrt((dstlat * dstlat) + (dstlon * dstlon)); + } + } + + public float ELToMAV + { + get + { + float dist = DistToMAV; + + float altdiff = (float)(alt - HomeLocation.Alt); + + float angle = (float)Math.Atan(altdiff / dist) * rad2deg; + + return angle; + } + } + + public float AZToMAV + { + get + { + // shrinking factor for longitude going to poles direction + double rads = Math.Abs(HomeLocation.Lat) * 0.0174532925; + double scaleLongDown = Math.Cos(rads); + double scaleLongUp = 1.0f / Math.Cos(rads); + + //DIR to Home + double dstlon = (HomeLocation.Lng - lng); //OffSet_X + double dstlat = (HomeLocation.Lat - lat) * scaleLongUp; //OffSet Y + double bearing = 90 + (Math.Atan2(dstlat, -dstlon) * 57.295775); //absolut home direction + if (bearing < 0) bearing += 360;//normalization + //bearing = bearing - 180;//absolut return direction + //if (bearing < 0) bearing += 360;//normalization + + return (float)bearing; + } + } + // current firmware public MainV2.Firmwares firmware = MainV2.Firmwares.ArduPlane; public float freemem { get; set; } @@ -658,6 +711,7 @@ namespace ArdupilotMega groundspeed = gps.v; groundcourse = gps.hdg; + //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null; } #endif @@ -853,7 +907,16 @@ namespace ArdupilotMega airspeed = vfr.airspeed; alt = vfr.alt; // this might include baro - + /* + if (vfr.throttle > 150 || vfr.throttle < 0) + { + Console.WriteLine(0); + } + else + { + Console.WriteLine(vfr.throttle); + } + */ //climbrate = vfr.climb; if ((DateTime.Now - lastalt).TotalSeconds >= 0.1 && oldalt != alt) diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index 922f587bec..31a96a2db7 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -74,6 +74,9 @@ namespace ArdupilotMega.GCSViews public static hud.HUD myhud = null; public static GMapControl mymap = null; + + PointLatLngAlt GuidedModeWP = new PointLatLngAlt(); + AviWriter aviwriter; public SplitContainer MainHcopy = null; @@ -440,7 +443,7 @@ namespace ArdupilotMega.GCSViews { if (plla == null || plla.Lng == 0 || plla.Lat == 0) break; - addpolygonmarker(plla.Tag, plla.Lng, plla.Lat, (int)plla.Alt,plla.color); + addpolygonmarker(plla.Tag, plla.Lng, plla.Lat, (int)plla.Alt,plla.color,polygons); } RegeneratePolygon(); @@ -448,6 +451,11 @@ namespace ArdupilotMega.GCSViews waypoints = DateTime.Now; } + if (MainV2.cs.mode.ToLower() == "guided" && GuidedModeWP != null && GuidedModeWP.Lat != 0) + { + addpolygonmarker("Guided Mode", GuidedModeWP.Lng, GuidedModeWP.Lat, (int)GuidedModeWP.Alt, Color.Blue, routes); + } + //routes.Polygons.Add(poly); if (trackPoints.Count > 0) @@ -482,8 +490,6 @@ namespace ArdupilotMega.GCSViews gMapControl1.Invalidate(); Application.DoEvents(); - - GC.Collect(); } tracklast = DateTime.Now; @@ -533,7 +539,7 @@ namespace ArdupilotMega.GCSViews }); } - private void addpolygonmarker(string tag, double lng, double lat, int alt, Color? color) + private void addpolygonmarker(string tag, double lng, double lat, int alt, Color? color, GMapOverlay overlay) { try { @@ -559,8 +565,8 @@ namespace ArdupilotMega.GCSViews } } - polygons.Markers.Add(m); - polygons.Markers.Add(mBorders); + overlay.Markers.Add(m); + overlay.Markers.Add(mBorders); } catch (Exception) { } } @@ -861,6 +867,8 @@ namespace ArdupilotMega.GCSViews MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT, (byte)2); + GuidedModeWP = new PointLatLngAlt(gotohere.lat, gotohere.lng, gotohere.alt,"Guided Mode"); + MainV2.givecomport = false; } catch (Exception ex) { MainV2.givecomport = false; MessageBox.Show("Error sending command : " + ex.Message); } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index 744dc9ac10..ff41e9046f 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -41,7 +41,6 @@ namespace ArdupilotMega.GCSViews private Dictionary cmdParamNames = new Dictionary(); - /// /// Reads defines.h for all valid commands and eeprom positions /// @@ -645,13 +644,13 @@ namespace ArdupilotMega.GCSViews config(false); - if (MainV2.HomeLocation.Lat != 0 && MainV2.HomeLocation.Lng != 0) + if (MainV2.cs.HomeLocation.Lat != 0 && MainV2.cs.HomeLocation.Lng != 0) { - TXT_homelat.Text = MainV2.HomeLocation.Lat.ToString(); + TXT_homelat.Text = MainV2.cs.HomeLocation.Lat.ToString(); - TXT_homelng.Text = MainV2.HomeLocation.Lng.ToString(); + TXT_homelng.Text = MainV2.cs.HomeLocation.Lng.ToString(); - TXT_homealt.Text = MainV2.HomeLocation.Alt.ToString(); + TXT_homealt.Text = MainV2.cs.HomeLocation.Alt.ToString(); } @@ -1685,7 +1684,7 @@ namespace ArdupilotMega.GCSViews sethome = false; try { - MainV2.HomeLocation.Lat = double.Parse(TXT_homelat.Text); + MainV2.cs.HomeLocation.Lat = double.Parse(TXT_homelat.Text); } catch { } writeKML(); @@ -1697,7 +1696,7 @@ namespace ArdupilotMega.GCSViews sethome = false; try { - MainV2.HomeLocation.Lng = double.Parse(TXT_homelng.Text); + MainV2.cs.HomeLocation.Lng = double.Parse(TXT_homelng.Text); } catch { } writeKML(); @@ -1708,7 +1707,7 @@ namespace ArdupilotMega.GCSViews sethome = false; try { - MainV2.HomeLocation.Alt = double.Parse(TXT_homealt.Text); + MainV2.cs.HomeLocation.Alt = double.Parse(TXT_homealt.Text); } catch { } writeKML(); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs index b58ee59038..50a4a64515 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs @@ -648,8 +648,8 @@ namespace ArdupilotMega.GCSViews //JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set position/h-agl-ft 0\r\n")); - JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set position/lat-gc-deg " + MainV2.HomeLocation.Lat + "\r\n")); - JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set position/long-gc-deg " + MainV2.HomeLocation.Lng + "\r\n")); + JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set position/lat-gc-deg " + MainV2.cs.HomeLocation.Lat + "\r\n")); + JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set position/long-gc-deg " + MainV2.cs.HomeLocation.Lng + "\r\n")); JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set attitude/phi-rad 0\r\n")); JSBSimSEND.Client.Send(System.Text.Encoding.ASCII.GetBytes("set attitude/theta-rad 0\r\n")); diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index d7d0a6def3..eeab0ed1c8 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -33,11 +33,6 @@ namespace ArdupilotMega const int SW_SHOWNORMAL = 1; const int SW_HIDE = 0; - /// - /// Home Location - /// - public static PointLatLngAlt HomeLocation = new PointLatLngAlt(); - public static MAVLink comPort = new MAVLink(); public static string comportname = ""; public static Hashtable config = new Hashtable(); @@ -208,13 +203,13 @@ namespace ArdupilotMega try { if (config["TXT_homelat"] != null) - HomeLocation.Lat = double.Parse(config["TXT_homelat"].ToString()); + cs.HomeLocation.Lat = double.Parse(config["TXT_homelat"].ToString()); if (config["TXT_homelng"] != null) - HomeLocation.Lng = double.Parse(config["TXT_homelng"].ToString()); + cs.HomeLocation.Lng = double.Parse(config["TXT_homelng"].ToString()); if (config["TXT_homealt"] != null) - HomeLocation.Alt = double.Parse(config["TXT_homealt"].ToString()); + cs.HomeLocation.Alt = double.Parse(config["TXT_homealt"].ToString()); } catch { } diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 33b541e6a2..7b31841394 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.1.35")] +[assembly: AssemblyFileVersion("1.1.36")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs index 22b7646398..7e90bfc1c5 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs @@ -1401,6 +1401,8 @@ namespace ArdupilotMega.Setup { timer.Stop(); timer.Dispose(); + + tabControl1.SelectedIndex = 0; } private void CHK_enableoptflow_CheckedChanged(object sender, EventArgs e) diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb index a07fdc2e56..6046b97ded 100644 Binary files a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb and b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb differ