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 InitAttitude()
{
_mavlink.Rtt.RawAttitude.Select(_ => (double)GeoMath.RadiansToDegrees(_.Pitch)).Subscribe(_pitch, DisposeCancel.Token);
_mavlink.Rtt.RawAttitude.Select(_ => (double)GeoMath.RadiansToDegrees(_.Roll)).Subscribe(_roll, DisposeCancel.Token);
_mavlink.Rtt.RawAttitude.Select(_ => (double)GeoMath.RadiansToDegrees(_.Yaw)).Subscribe(_yaw, DisposeCancel.Token);
_mavlink.Rtt.RawAttitude.Select(_ => (double)_.Pitchspeed).Subscribe(_pitchSpeed, DisposeCancel.Token);
_mavlink.Rtt.RawAttitude.Select(_ => (double)_.Rollspeed).Subscribe(_rollSpeed, DisposeCancel.Token);
_mavlink.Rtt.RawAttitude.Select(_ => (double)_.Yawspeed).Subscribe(_yawSpeed, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _pitch.Dispose());
DisposeCancel.Token.Register(() => _roll.Dispose());
DisposeCancel.Token.Register(() => _yaw.Dispose());
DisposeCancel.Token.Register(() => _pitchSpeed.Dispose());
DisposeCancel.Token.Register(() => _rollSpeed.Dispose());
DisposeCancel.Token.Register(() => _yawSpeed.Dispose());
}