public GMapMarkerPhoto(MAVLink.mavlink_camera_feedback_t mark, bool shotBellowMinInterval = false)
            : base(new PointLatLng(mark.lat / 1e7, mark.lng / 1e7))
        {
            local = mark;
            this.shotBellowMinInterval = shotBellowMinInterval;
            Offset      = new Point(-localcache1.Width / 2, -localcache1.Height / 2);
            Size        = localcache1.Size;
            Alt         = mark.alt_msl;
            ToolTipMode = MarkerTooltipMode.OnMouseOver;

            Roll  = local.roll - rolltrim;
            Pitch = local.pitch - pitchtrim;
            Yaw   = local.yaw - yawtrim;

            ToolTipText = "Photo" + "\nAlt: " + mark.alt_msl + "\nNo: " + mark.img_idx + "\nRoll: " + Roll.ToString("0.00");

            Tag = mark.time_usec;

            var footprint = ImageProjection.calc(new PointLatLngAlt(Position.Lat, Position.Lng, Alt), Roll,
                                                 Pitch, Yaw, hfov, vfov);

            footprintpoly = new GMapPolygon(footprint.ConvertAll(x => x.Point()), "FP" + mark.time_usec);

            footprintpoly.Fill   = Brushes.Transparent;
            footprintpoly.Stroke = Pens.Crimson;
        }
Example #2
0
        private void domainUpDown1_ValueChanged(object sender, EventArgs e)
        {
            if (loading)
            {
                return;
            }

            if (CMB_camera.Text != "")
            {
                doCalc();
            }

            // new grid system test

            grid = Grid.CreateGrid(list, CurrentState.fromDistDisplayUnit((double)NUM_altitude.Value),
                                   (double)NUM_Distance, (double)NUM_spacing, (double)NUM_angle.Value,
                                   (double)NUM_overshoot, (double)NUM_overshoot2,
                                   (Grid.StartPosition)Enum.Parse(typeof(Grid.StartPosition), CMB_startfrom.Text), false,
                                   (float)NUM_Lane_Dist, (float)NUM_leadin);

            map.HoldInvalidation = true;

            routesOverlay.Routes.Clear();
            routesOverlay.Polygons.Clear();
            routesOverlay.Markers.Clear();

            GMapMarkerOverlap.Clear();

            if (grid.Count == 0)
            {
                return;
            }

            if (chk_crossgrid.Checked)
            {
                // add crossover
                Grid.StartPointLatLngAlt = grid[grid.Count - 1];

                grid.AddRange(Grid.CreateGrid(list, CurrentState.fromDistDisplayUnit((double)NUM_altitude.Value),
                                              (double)NUM_Distance, (double)NUM_spacing, (double)NUM_angle.Value + 90.0,
                                              (double)NUM_overshoot, (double)NUM_overshoot2,
                                              Grid.StartPosition.Point, false,
                                              (float)NUM_Lane_Dist, (float)NUM_leadin));
            }

            //if (CHK_boundary.Checked)
            AddDrawPolygon();

            int                strips             = 0;
            int                images             = 0;
            int                a                  = 1;
            PointLatLngAlt     prevpoint          = grid[0];
            float              routetotal         = 0;
            List <PointLatLng> segment            = new List <PointLatLng>();
            double             maxgroundelevation = double.MinValue;
            double             mingroundelevation = double.MaxValue;
            double             startalt           = plugin.Host.cs.HomeAlt;

            foreach (var item in grid)
            {
                double currentalt = srtm.getAltitude(item.Lat, item.Lng).alt;
                mingroundelevation = Math.Min(mingroundelevation, currentalt);
                maxgroundelevation = Math.Max(maxgroundelevation, currentalt);

                if (item.Tag == "M")
                {
                    images++;

                    //if (CHK_internals.Checked)   MP中未直接打勾的就先暂时认为是不用选中
                    //{
                    //    routesOverlay.Markers.Add(new GMarkerGoogle(item, GMarkerGoogleType.green) { ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.OnMouseOver });
                    //    a++;

                    //    segment.Add(prevpoint);
                    //    segment.Add(item);
                    //    prevpoint = item;
                    //}
                    try
                    {
                        if (TXT_fovH != "")
                        {
                            if (CHK_footprints.Checked)
                            {
                                double fovh = double.Parse(TXT_fovH);
                                double fovv = double.Parse(TXT_fovV);

                                getFOV(item.Alt + startalt - currentalt, ref fovh, ref fovv);

                                double startangle = 0;

                                //if (!CHK_camdirection.Checked)   MP中直接打勾的视为必选项
                                //{
                                startangle = 90;
                                //}

                                double angle1 = startangle - (Math.Sin((fovh / 2.0) / (fovv / 2.0)) * rad2deg);
                                double dist1  = Math.Sqrt(Math.Pow(fovh / 2.0, 2) + Math.Pow(fovv / 2.0, 2));

                                double bearing = (double)NUM_angle.Value;

                                //if (CHK_copter_headinghold.Checked)
                                //{
                                //    bearing = Convert.ToInt32(TXT_headinghold);
                                //}

                                double fovha = 0;
                                double fovva = 0;
                                getFOVangle(ref fovha, ref fovva);
                                var itemcopy = new PointLatLngAlt(item);
                                itemcopy.Alt += startalt;
                                var temp = ImageProjection.calc(itemcopy, 0, 0, bearing + startangle, fovha, fovva);

                                List <PointLatLng> footprint = new List <PointLatLng>();
                                footprint.Add(temp[0]);
                                footprint.Add(temp[1]);
                                footprint.Add(temp[2]);
                                footprint.Add(temp[3]);

                                GMapPolygon poly = new GMapPolygon(footprint, a.ToString());
                                poly.Stroke =
                                    new Pen(Color.FromArgb(250 - ((a * 5) % 240), 250 - ((a * 3) % 240), 250 - ((a * 9) % 240)), 1);
                                poly.Fill = new SolidBrush(Color.Transparent);

                                GMapMarkerOverlap.Add(poly);

                                routesOverlay.Polygons.Add(poly);
                                a++;
                            }
                        }
                    }
                    catch { }
                }
                else
                {
                    if (item.Tag != "SM" && item.Tag != "ME")
                    {
                        strips++;
                    }

                    if (CHK_markers.Checked)
                    {
                        var marker = new GMapMarkerWP(item, a.ToString())
                        {
                            ToolTipText = a.ToString(), ToolTipMode = MarkerTooltipMode.OnMouseOver
                        };
                        routesOverlay.Markers.Add(marker);
                    }

                    segment.Add(prevpoint);
                    segment.Add(item);
                    prevpoint = item;
                    a++;
                }
                GMapRoute seg = new GMapRoute(segment, "segment" + a.ToString());
                seg.Stroke           = new Pen(Color.Yellow, 4);
                seg.Stroke.DashStyle = System.Drawing.Drawing2D.DashStyle.Custom;
                seg.IsHitTestVisible = true;
                routetotal           = routetotal + (float)seg.Distance;
                //if (CHK_grid)
                //{
                routesOverlay.Routes.Add(seg);
                //}
                //else
                //{
                //    seg.Dispose();
                //}

                segment.Clear();
            }

            if (CHK_footprints.Checked)
            {
                routesOverlay.Markers.Add(GMapMarkerOverlap);
            }

            /*      Old way of drawing route, incase something breaks using segments
             * GMapRoute wproute = new GMapRoute(list2, "GridRoute");
             * wproute.Stroke = new Pen(Color.Yellow, 4);
             * if (chk_grid.Checked)
             *  routesOverlay.Routes.Add(wproute);
             */

            // turn radrad = tas^2 / (tan(angle) * G)
            // float v_sq = (float)(((float)NUM_UpDownFlySpeed.Value / CurrentState.multiplierspeed) * ((float)NUM_UpDownFlySpeed.Value / CurrentState.multiplierspeed));
            // float turnrad = (float)(v_sq / (float)(9.808f * Math.Tan(35 * deg2rad)));

            // Update Stats
            if (DistUnits == "Feet")
            {
                // Area
                float area = (float)calcpolygonarea(list) * 10.7639f; // Calculate the area in square feet
                lbl_area.Text = area.ToString("#") + " ft^2";
                if (area < 21780f)
                {
                    lbl_area.Text = area.ToString("#") + " ft^2";
                }
                else
                {
                    area = area / 43560f;
                    if (area < 640f)
                    {
                        lbl_area.Text = area.ToString("0.##") + " acres";
                    }
                    else
                    {
                        area          = area / 640f;
                        lbl_area.Text = area.ToString("0.##") + " miles^2";
                    }
                }

                // Distance
                float distance = routetotal * 3280.84f; // Calculate the distance in feet
                if (distance < 5280f)
                {
                    lbl_distance.Text = distance.ToString("#") + " ft";
                }
                else
                {
                    distance          = distance / 5280f;
                    lbl_distance.Text = distance.ToString("0.##") + " miles";
                }

                lbl_spacing.Text          = (NUM_spacing * 3.2808399m).ToString("#") + " ft";
                lbl_grndres.Text          = inchpixel;
                lbl_distbetweenlines.Text = (NUM_Distance * 3.2808399m).ToString("0.##") + " ft";
                lbl_footprint.Text        = feet_fovH + " x " + feet_fovV + " ft";
                //lbl_turnrad.Text = (turnrad * 2 * 3.2808399).ToString("0") + " ft";
                lbl_gndelev.Text = (mingroundelevation * 3.2808399).ToString("0") + "-" + (maxgroundelevation * 3.2808399).ToString("0") + " ft";
            }
            else
            {
                // Meters
                lbl_area.Text             = calcpolygonarea(list).ToString("#") + " m^2";
                lbl_distance.Text         = routetotal.ToString("0.##") + " km";
                lbl_spacing.Text          = NUM_spacing.ToString("#") + " m";
                lbl_grndres.Text          = TXT_cmpixel;
                lbl_distbetweenlines.Text = NUM_Distance.ToString("0.##") + " m";
                lbl_footprint.Text        = TXT_fovH + " x " + TXT_fovV + " m";
                //lbl_turnrad.Text = (turnrad * 2).ToString("0") + " m";
                lbl_gndelev.Text = mingroundelevation.ToString("0") + "-" + maxgroundelevation.ToString("0") + " m";
            }

            try
            {
                // speed m/s
                var speed = ((float)NUM_UpDownFlySpeed / CurrentState.multiplierspeed);
                // cmpix cm/pixel
                var cmpix = float.Parse(TXT_cmpixel.TrimEnd(new[] { 'c', 'm', ' ' }));
                // m pix = m/pixel
                var mpix = cmpix * 0.01;
                // gsd / 2.0
                var minmpix = mpix / 2.0;
                // min sutter speed
                var minshutter = speed / minmpix;
                //lbl_minshutter.Text = "1/" + (minshutter - minshutter % 1).ToString();
            }
            catch { }

            double flyspeedms = CurrentState.fromSpeedDisplayUnit((double)NUM_UpDownFlySpeed);

            lbl_pictures.Text = images.ToString();
            lbl_strips.Text   = ((int)(strips / 2)).ToString();
            double seconds = ((routetotal * 1000.0) / ((flyspeedms) * 0.8));

            // reduce flying speed by 20 %
            lbl_flighttime.Text  = secondsToNice(seconds);
            seconds              = ((routetotal * 1000.0) / (flyspeedms));
            lbl_photoevery.Text  = secondsToNice(((double)NUM_spacing / flyspeedms));
            map.HoldInvalidation = false;
            if (!isMouseDown && sender != NUM_angle)
            {
                map.ZoomAndCenterMarkers("routes");
            }

            CalcHeadingHold();

            map.Invalidate();
        }