private async Task <double> GoToPointUntilReachAzimuth(GeoPoint point, double azimuth, double precisionMet, double precisionDegr, CancellationToken cancel, int attemptsCnt = 1)
{
_logger.Info($"GoTo point {point}");
await GoToGlobAndWait(point, CallbackProgress <double> .Default, precisionMet, cancel).ConfigureAwait(false);
_logger.Info("Start circling");
var attempts = 0;
while (!cancel.IsCancellationRequested)
{
var location = GlobalPosition.Value;
var currentAzimuth = point.Azimuth(location);
_logger.Info($"Azimuth relative to the point {currentAzimuth:F1} deg");
if (IsInAzimuthLimits(currentAzimuth, azimuth, precisionDegr))
{
attempts++;
if (attempts >= attemptsCnt)
{
return(GeoMath.Distance(location, point));
}
await Task.Delay(TimeSpan.FromMilliseconds(1500), cancel).ConfigureAwait(false);
}
await Task.Delay(TimeSpan.FromMilliseconds(250), cancel).ConfigureAwait(false);
}
return(0);
}
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);
}
protected virtual void InitHome()
{
_mavlink.Rtt.RawHome
.Select(_ => (GeoPoint?)new GeoPoint(_.Latitude / 10000000D, _.Longitude / 10000000D, _.Altitude / 1000D))
.Subscribe(_home, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _homeDistance.Dispose());
this.GlobalPosition
.Where(_ => _home.Value.HasValue)
// ReSharper disable once PossibleInvalidOperationException
.Select(_ => (double?)GeoMath.Distance(_home.Value.Value, _))
.Subscribe(_homeDistance, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _home.Dispose());
}
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 static async Task DoRepositionAndWait(this IVehicle vehicle, GeoPoint geoPoint, double velocity, double precisionMet, int checkTimeMs, CancellationToken cancel, IProgress <double> progress)
{
progress = progress ?? new Progress <double>();
var startLocation = vehicle.Rtt.RelGps.Value;
var startDistance = GeoMath.Distance(geoPoint, startLocation);
Logger.Info("DoRepositionAndWait {0} with V={1:F1} m/sec and precision {2:F1} m. Distance to target {3:F1}", geoPoint, velocity, precisionMet, startDistance);
progress.Report(0);
if (startDistance <= precisionMet)
{
Logger.Debug("Already in target, nothing to do", startLocation);
progress.Report(1);
return;
}
var sw = new Stopwatch();
sw.Start();
Logger.Debug("Send command DoReposition to vehicle", startLocation);
await vehicle.DoReposition((float)velocity, geoPoint, cancel).ConfigureAwait(false);
double dist = 0;
while (!cancel.IsCancellationRequested)
{
var loc = vehicle.Rtt.RelGps.Value;
dist = Math.Abs(GeoMath.Distance(geoPoint, loc));
var prog = 1 - dist / startDistance;
Logger.Trace("Distance to target {0:F1}, location: {1}, progress {2:P2}", dist, loc, prog);
progress.Report(prog);
if (dist <= precisionMet)
{
break;
}
await Task.Delay(checkTimeMs, cancel).ConfigureAwait(false);
}
sw.Stop();
Logger.Info($"Complete {sw.Elapsed:hh\\:mm\\:ss} location error {dist:F1} m");
progress.Report(1);
}
public static double DistanceTo(this GeoPoint a, GeoPoint b)
{
return(GeoMath.Distance(a, b));
}
public double DistanceTo(GeoPoint other)
{
return(GeoMath.Distance(this, other));
}