public async Task GoToGlobAndWaitWithoutAltitude(GeoPoint location, IProgress <double> progress, double precisionMet, CancellationToken cancel) { await GoToGlob(location, cancel); progress = progress ?? new Progress <double>(); var startLocation = this.GlobalPosition.Value; var startLocation0 = startLocation.SetAltitude(0); var startDistance = Math.Abs(GeoMath.Distance(location, startLocation)); var startDistance0 = Math.Abs(GeoMath.Distance(location.SetAltitude(0), startLocation0)); Logger.Info("GoToAndWaitWithoutAltitude {0} with precision {2:F1} m. Distance to target {3:F1}", location, precisionMet, startDistance); progress.Report(0); if (startDistance0 <= precisionMet) { Logger.Debug("Already in target, nothing to do", startLocation); progress.Report(1); return; } var sw = new Stopwatch(); sw.Start(); Logger.Debug("Send command GoTo to vehicle", startLocation); await this.GoToGlob(location, cancel).ConfigureAwait(false); double dist = 0; while (!cancel.IsCancellationRequested) { var loc = this.GlobalPosition.Value; var loc0 = loc.SetAltitude(0); dist = Math.Abs(GeoMath.Distance(location, loc)); var dist0 = Math.Abs(GeoMath.Distance(location.SetAltitude(0), loc0)); var prog = 1 - dist / startDistance; Logger.Trace("Distance to target {0:F1}, location: {1}, progress {2:P2}", dist, loc, prog); progress.Report(prog); if (dist0 <= precisionMet) { break; } await Task.Delay(TimeSpan.FromSeconds(1), cancel).ConfigureAwait(false); } sw.Stop(); Logger.Info($"Complete {sw.Elapsed:hh\\:mm\\:ss} location error {dist:F1} m"); progress.Report(1); }
public override async Task FlyByLineGlob(GeoPoint start, GeoPoint stop, double precisionMet, CancellationToken cancel, Action firstPointComplete = null) { const int ApproachAngle = 90; const int PrePointDistanceParts = 2; const double GoToPrecisionErrorMet = 20.5; var waitForDetectingAzimuthTime = TimeSpan.FromSeconds(3); var waitInPrePointTime = TimeSpan.FromSeconds(3); var waitForCorrectionTime = TimeSpan.FromSeconds(1); var start0 = start.SetAltitude(0); var stop0 = stop.SetAltitude(0); var azimuth = start0.Azimuth(stop0); var reverseAzimuth = stop0.Azimuth(start0); var loiterRadius = (_planeRadius.Value ?? await _planeRadius.FirstAsync(_ => _.HasValue)).Value; var realLoiterRadius = (int)(loiterRadius + GoToPrecisionErrorMet); var startPrePoint0 = start0.RadialPoint(PrePointDistanceParts * realLoiterRadius, reverseAzimuth); var pointDistanceFromGround = GeoMath.Distance(start0, stop0); var angle = Math.Abs(pointDistanceFromGround) > 0.001 ? GeoMath.RadiansToDegrees(Math.Atan(Math.Abs((start.Altitude) - (stop.Altitude)) / pointDistanceFromGround)) : 0.0; var height = GeoMath.HeightFromGroundRange(GeoMath.Distance(stop0, startPrePoint0), angle); if ((start.Altitude) - (stop.Altitude) < 0) { height = (stop.Altitude) - height; } else { height = (stop.Altitude) + height; } //TODO if (height <= 0) нужно что-то делать!!! if (height <= 0) { _logger.Info($"Impossible fly by line from altitude {start.Altitude}м to altitude {stop.Altitude}m"); return; } var firstPrePoint = startPrePoint0.RadialPoint(PrePointDistanceParts * realLoiterRadius, azimuth + ApproachAngle).SetAltitude(height); var r = await GoToPointUntilReachAzimuth(firstPrePoint, reverseAzimuth - 10, realLoiterRadius, 4, cancel, 2).ConfigureAwait(false); var secondPrePoint = startPrePoint0.RadialPoint(r, azimuth + ApproachAngle).SetAltitude(height); await GoToPointUntilReachAzimuth(secondPrePoint, reverseAzimuth + ApproachAngle - 10, realLoiterRadius, 2, cancel, 2).ConfigureAwait(false); await GoToGlobAndWait(stop, CallbackProgress <double> .Default, realLoiterRadius, cancel).ConfigureAwait(false); }