735 lines
28 KiB
C#
735 lines
28 KiB
C#
using Plane.Communication;
|
||
using Plane.Copters;
|
||
using Plane.Logging;
|
||
using Newtonsoft.Json;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Collections.Concurrent;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
using System.Threading;
|
||
using System.Diagnostics;
|
||
using static Plane.Copters.Constants;
|
||
using Plane.Geography;
|
||
|
||
namespace Plane.FormationCreator.Formation
|
||
{
|
||
public class FormationController
|
||
{
|
||
public FormationController(ILogger logger, CopterManager copterManager)
|
||
{
|
||
_logger = logger;
|
||
_copterManager = copterManager;
|
||
|
||
foreach (var copter in _copterManager.Copters)
|
||
{
|
||
_guides.Add(copter, null);
|
||
}
|
||
_copterManager.Copters.CollectionChanged += (sender, e) =>
|
||
{
|
||
if (e.OldItems != null)
|
||
{
|
||
foreach (ICopter copter in e.OldItems)
|
||
{
|
||
if (_guides.ContainsKey(copter))
|
||
{
|
||
_guides.Remove(copter);
|
||
}
|
||
}
|
||
}
|
||
if (e.NewItems != null)
|
||
{
|
||
foreach (ICopter copter in e.NewItems)
|
||
{
|
||
if (!_guides.ContainsKey(copter))
|
||
{
|
||
_guides.Add(copter, null);
|
||
}
|
||
}
|
||
}
|
||
};
|
||
//var thread = new Thread(() =>
|
||
//{
|
||
// while (true)
|
||
// {
|
||
// Control();
|
||
// Thread.Sleep(50);
|
||
// }
|
||
//})
|
||
//{
|
||
// IsBackground = true
|
||
//};
|
||
//thread.Start();
|
||
var timer = new System.Timers.Timer(INTERVAL);
|
||
timer.Elapsed += (sender, e) => Control();
|
||
timer.Start();
|
||
}
|
||
|
||
private const int INTERVAL = 50;
|
||
|
||
private bool _shouldStop;
|
||
private Dictionary<ICopter, Guide> _guides = new Dictionary<ICopter, Guide>();
|
||
|
||
private ILogger _logger;
|
||
private CopterManager _copterManager;
|
||
|
||
#region 检查是否有飞行器在某状态的方法
|
||
|
||
private bool AnyCopterWithGuide()
|
||
{
|
||
try
|
||
{
|
||
return _guides.Any(item => item.Value != null);
|
||
}
|
||
catch (InvalidOperationException ex) // 集合已修改。
|
||
{
|
||
_logger.Log(ex.ToString(), Category.Info, Priority.None);
|
||
return true; // 安全起见返回 true，让调用者再次检查。
|
||
}
|
||
}
|
||
|
||
private bool AnyCopterWithGuide(IEnumerable<ICopter> copters)
|
||
{
|
||
try
|
||
{
|
||
return copters.Any(c => _guides[c] != null);
|
||
}
|
||
catch (InvalidCastException ex) // 集合已修改。
|
||
{
|
||
_logger.Log(ex.ToString(), Category.Info, Priority.None);
|
||
return true; // 安全起见返回 true，让调用者再次检查。
|
||
}
|
||
}
|
||
|
||
private async Task EnsureAllTasksFinishedAsync()
|
||
{
|
||
await Task.Run(async () =>
|
||
{
|
||
while (AnyCopterWithGuide())
|
||
{
|
||
await Task.Delay(INTERVAL).ConfigureAwait(false);
|
||
}
|
||
}).ConfigureAwait(false);
|
||
}
|
||
|
||
private async Task EnsureCopterTasksFinishedAsync(IEnumerable<ICopter> copters)
|
||
{
|
||
await Task.Run(async () =>
|
||
{
|
||
while (AnyCopterWithGuide(copters))
|
||
{
|
||
await Task.Delay(INTERVAL).ConfigureAwait(false);
|
||
}
|
||
}).ConfigureAwait(false);
|
||
}
|
||
|
||
private async Task EnsureTaskFinishedAsync(ICopter copter)
|
||
{
|
||
while (_guides[copter] != null)
|
||
{
|
||
await Task.Delay(INTERVAL).ConfigureAwait(false);
|
||
}
|
||
}
|
||
|
||
#endregion 检查是否有飞行器在某状态的方法
|
||
|
||
private void Control()
|
||
{
|
||
try
|
||
{
|
||
var copters = _guides.Keys.ToList();
|
||
|
||
Parallel.ForEach(copters, (copter, loopState) =>
|
||
{
|
||
try
|
||
{
|
||
if (_shouldStop)
|
||
{
|
||
loopState.Stop();
|
||
return;
|
||
}
|
||
var guide = _guides[copter];
|
||
if (guide == null)
|
||
{
|
||
return;
|
||
}
|
||
if (!guide.IsLoop && guide.FinishPredicate?.Invoke() == true)
|
||
{
|
||
var finishAction = guide?.FinishAction;
|
||
_guides[copter] = null;
|
||
copter.HoverAsync().Wait();
|
||
finishAction?.Invoke();
|
||
}
|
||
else
|
||
{
|
||
guide.UpdateAction?.Invoke();
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.Log(ex);
|
||
}
|
||
});
|
||
|
||
if (_shouldStop)
|
||
{
|
||
Parallel.ForEach(copters, copter =>
|
||
{
|
||
try
|
||
{
|
||
var finishAction = _guides[copter]?.FinishAction;
|
||
_guides[copter] = null;
|
||
copter.HoverAsync().Wait();
|
||
finishAction?.Invoke();
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.Log(ex);
|
||
}
|
||
});
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.Log(ex);
|
||
}
|
||
}
|
||
|
||
public async Task AllStop()
|
||
{
|
||
Parallel.ForEach(_copterManager.Copters, copter => copter.HoverAsync());
|
||
|
||
if (!_shouldStop && AnyCopterWithGuide())
|
||
{
|
||
_shouldStop = true;
|
||
while (AnyCopterWithGuide())
|
||
{
|
||
await Task.Delay(INTERVAL).ConfigureAwait(false);
|
||
}
|
||
_shouldStop = false;
|
||
}
|
||
|
||
Parallel.ForEach(_copterManager.Copters, copter => copter.HoverAsync().Wait());
|
||
}
|
||
|
||
public async Task TurnAsync(IEnumerable<ICopter> copters, double targetLat, double targetLng)
|
||
{
|
||
await AllStop();
|
||
|
||
foreach (var copter in copters)
|
||
{
|
||
var guide = !_guides.ContainsKey(copter) || _guides[copter] == null ? (_guides[copter] = new Guide()) : _guides[copter];
|
||
var targetHeading = (float)GeographyUtils.RadToDeg(GeographyUtils.CalcDirection2D(copter.Latitude, copter.Longitude, targetLat, targetLng));
|
||
guide.FinishPredicate = () => Math.Abs(copter.Heading - targetHeading) < 1;
|
||
guide.UpdateAction = () =>
|
||
{
|
||
copter.MoveToIdealState(new CopterState
|
||
{
|
||
Heading = targetHeading
|
||
});
|
||
};
|
||
}
|
||
|
||
await EnsureAllTasksFinishedAsync().ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task FlyToLatLngAsync(ICopter copter, double targetLat, double targetLng)
|
||
{
|
||
const double DT = INTERVAL / 1000.0;
|
||
const double DECELERATE_DISTANCE = 15;
|
||
double idealVel = 0;
|
||
double idealMovedDistance = 0;
|
||
var startLat = copter.Latitude;
|
||
var startLng = copter.Longitude;
|
||
var startTargetDistance = GeographyUtils.CalcDistance2D(startLat, startLng, targetLat, targetLng);
|
||
var idealMoveDirectionRadians = (float)GeographyUtils.CalcDirection2D(startLat, startLng, targetLat, targetLng);
|
||
var idealMoveDirectionDegrees = (float)GeographyUtils.RadToDeg(idealMoveDirectionRadians);
|
||
|
||
var guide = !_guides.ContainsKey(copter) || _guides[copter] == null ? (_guides[copter] = new Guide()) : _guides[copter];
|
||
guide.FinishPredicate = () => GeographyUtils.CalcDistance2D(copter.Latitude, copter.Longitude, targetLat, targetLng) <= 0.5;
|
||
var log = new List<object>();
|
||
guide.UpdateAction = () =>
|
||
{
|
||
if (idealVel < MAX_VEL)
|
||
{
|
||
idealVel += MAX_ACCEL * DT;
|
||
if (idealVel > MAX_VEL) idealVel = MAX_VEL;
|
||
}
|
||
var distance = GeographyUtils.CalcDistance2D(copter.Latitude, copter.Longitude, targetLat, targetLng);
|
||
if (distance < DECELERATE_DISTANCE) idealVel = MAX_VEL * distance / DECELERATE_DISTANCE;
|
||
|
||
double idealLat, idealLng;
|
||
if (idealMovedDistance < startTargetDistance)
|
||
{
|
||
idealMovedDistance += idealVel * DT;
|
||
if (idealMovedDistance > startTargetDistance)
|
||
{
|
||
idealLat = targetLat;
|
||
idealLng = targetLng;
|
||
}
|
||
else
|
||
{
|
||
var idealLatLng = GeographyUtils.CalcLatLngSomeMetersAway2D(startLat, startLng, idealMoveDirectionDegrees, (float)idealMovedDistance);
|
||
idealLat = idealLatLng.Item1;
|
||
idealLng = idealLatLng.Item2;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
idealLat = targetLat;
|
||
idealLng = targetLng;
|
||
}
|
||
|
||
copter.MoveToIdealState(new CopterState
|
||
{
|
||
Latitude = idealLat,
|
||
Longitude = idealLng,
|
||
Heading = idealMoveDirectionDegrees,
|
||
VelocityDirection = idealMoveDirectionRadians,
|
||
Velocity = idealVel
|
||
});
|
||
|
||
log.Add(new
|
||
{
|
||
time = DateTime.Now,
|
||
targetLat = idealLat,
|
||
targetLng = idealLng,
|
||
lat = copter.Latitude,
|
||
lng = copter.Longitude,
|
||
alt = copter.Altitude
|
||
});
|
||
};
|
||
guide.FinishAction = () => _logger.Log(JsonConvert.SerializeObject(log));
|
||
|
||
await EnsureAllTasksFinishedAsync().ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task FlyToLatLngAsync(IEnumerable<ICopter> copters, double targetLat, double targetLng)
|
||
{
|
||
await AllStop();
|
||
|
||
if (!copters.Any()) return;
|
||
|
||
var centerLat = copters.Average(c => c.Latitude);
|
||
var centerLng = copters.Average(c => c.Longitude);
|
||
var latDelta = targetLat - centerLat;
|
||
var lngDelta = targetLng - centerLng;
|
||
|
||
foreach (var copter in copters)
|
||
{
|
||
var copterTargetLat = copter.Latitude + latDelta;
|
||
var copterTargetLng = copter.Longitude + lngDelta;
|
||
var guide = !_guides.ContainsKey(copter) || _guides[copter] == null ? (_guides[copter] = new Guide()) : _guides[copter];
|
||
guide.FinishPredicate = () => GeographyUtils.CalcDistance2D(copter.Latitude, copter.Longitude, copterTargetLat, copterTargetLng) <= 0.5;
|
||
guide.UpdateAction = () =>
|
||
{
|
||
copter.MoveToIdealState(new CopterState
|
||
{
|
||
Latitude = copterTargetLat,
|
||
Longitude = copterTargetLng
|
||
});
|
||
};
|
||
}
|
||
|
||
await EnsureAllTasksFinishedAsync().ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task FlyToAltitudeAsync(ICopter copter, float targetAlt)
|
||
{
|
||
var guide = _guides[copter] ?? (_guides[copter] = new Guide());
|
||
guide.FinishPredicate = () => Math.Abs(copter.Altitude - targetAlt) <= 0.5;
|
||
guide.UpdateAction = () =>
|
||
{
|
||
copter.MoveToIdealState(new CopterState
|
||
{
|
||
Altitude = targetAlt
|
||
});
|
||
};
|
||
|
||
await EnsureTaskFinishedAsync(copter).ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task FlyToAltitudeAsync(IEnumerable<ICopter> copters, float targetAlt)
|
||
{
|
||
await AllStop();
|
||
|
||
foreach (var copter in copters)
|
||
{
|
||
if (!_guides.ContainsKey(copter))
|
||
{
|
||
_guides.Add(copter, null);
|
||
}
|
||
var guide = _guides[copter] ?? (_guides[copter] = new Guide());
|
||
guide.FinishPredicate = () => Math.Abs(copter.Altitude - targetAlt) <= 0.5;
|
||
guide.UpdateAction = () =>
|
||
{
|
||
copter.MoveToIdealState(new CopterState
|
||
{
|
||
Altitude = targetAlt
|
||
});
|
||
};
|
||
}
|
||
|
||
await EnsureAllTasksFinishedAsync().ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task FlyInCircleAsync(IEnumerable<ICopter> copters, float centerDirection, float radius, bool clockwise = true, bool loop = false, double? velLimit = null)
|
||
{
|
||
await AllStop();
|
||
|
||
var velMax = velLimit ?? 2.0;
|
||
var velMin = -velMax;
|
||
var angVelMax = velMax / radius;
|
||
var angVelMin = velMin / radius;
|
||
var angAccel = (clockwise ? MAX_ACCEL : -MAX_ACCEL) / radius;
|
||
|
||
Parallel.ForEach(copters, copter =>
|
||
{
|
||
var guide = _guides[copter] ?? (_guides[copter] = new Guide());
|
||
guide.IsLoop = loop;
|
||
|
||
var centerLat = copter.Latitude + radius * Math.Cos(GeographyUtils.DegToRad(centerDirection)) * GeographyUtils.METERS_TO_LAT_SPAN;
|
||
var centerLng = copter.Longitude + radius * Math.Sin(GeographyUtils.DegToRad(centerDirection)) * GeographyUtils.CalcMetersToLngSpan(copter.Latitude);
|
||
|
||
var startingPointLat = copter.Latitude;
|
||
var startingPointLng = copter.Longitude;
|
||
|
||
var angVel = 0.0;
|
||
const double DT = INTERVAL / 1000.0;
|
||
var angleTotal = 0.0;
|
||
var startAngle = GeographyUtils.CalcDirection2D(centerLat, centerLng, copter.Latitude, copter.Longitude);
|
||
|
||
guide.FinishPredicate = () => angleTotal >= Math.PI * 2;
|
||
|
||
var log = new List<object>();
|
||
|
||
#region guide.UpdateAction
|
||
|
||
guide.UpdateAction = () =>
|
||
{
|
||
if (clockwise)
|
||
{
|
||
if (angVel < angVelMax)
|
||
{
|
||
angVel += angAccel * DT;
|
||
MathUtils.Constrain(ref angVel, 0, angVelMax);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (angVel > angVelMin)
|
||
{
|
||
angVel += angAccel * DT;
|
||
MathUtils.Constrain(ref angVel, angVelMin, 0);
|
||
}
|
||
}
|
||
|
||
var angleChange = angVel * DT;
|
||
angleTotal += angleChange;
|
||
var targetAngle = startAngle + angleTotal;
|
||
var targetLat = centerLat + Math.Cos(targetAngle) * radius * GeographyUtils.METERS_TO_LAT_SPAN;
|
||
var targetLng = centerLng + Math.Sin(targetAngle) * radius * GeographyUtils.CalcMetersToLngSpan(copter.Latitude);
|
||
|
||
copter.MoveToIdealState(new CopterState
|
||
{
|
||
Latitude = targetLat,
|
||
Longitude = targetLng,
|
||
Heading = (float)GeographyUtils.RadToDeg(GeographyUtils.CalcDirection2D(targetLat, targetLng, centerLat, centerLng)),
|
||
VelocityDirection = GeographyUtils.CalcDirection2D(centerLat, centerLng, targetLat, targetLng)
|
||
+ Math.PI / 2,
|
||
Velocity = angVel * radius
|
||
});
|
||
|
||
log.Add(new
|
||
{
|
||
time = DateTime.Now,
|
||
targetLat,
|
||
targetLng,
|
||
lat = copter.Latitude,
|
||
lng = copter.Longitude,
|
||
alt = copter.Altitude,
|
||
angAccel,
|
||
angVel,
|
||
angleChange,
|
||
angleTotal
|
||
});
|
||
};
|
||
|
||
#endregion guide.UpdateAction
|
||
|
||
guide.FinishAction = () =>
|
||
{
|
||
_logger.Log(JsonConvert.SerializeObject(log));
|
||
};
|
||
});
|
||
|
||
await EnsureAllTasksFinishedAsync().ConfigureAwait(false);
|
||
}
|
||
|
||
public async Task FlyAroundCenterOfCoptersAsync(IEnumerable<ICopter> copters, bool clockwise = true, bool loop = false, double? velLimit = null)
|
||
{
|
||
await AllStop();
|
||
|
||
var nullableCenter = copters.GetCenter();
|
||
if (nullableCenter == null) return;
|
||
|
||
var center = nullableCenter.Value;
|
||
|
||
var radiuses = copters.ToDictionary(c => c, c => GeographyUtils.CalcDistance2D(c.Latitude, c.Longitude, center.Lat, center.Lng));
|
||
var velMax = velLimit ?? 2.0;
|
||
var velMin = -velMax;
|
||
var angVelMax = velMax / radiuses.Values.Max();
|
||
|
||
Parallel.ForEach(copters, copter =>
|
||
{
|
||
var guide = _guides[copter] ?? (_guides[copter] = new Guide());
|
||
guide.IsLoop = loop;
|
||
|
||
var startingPointLat = copter.Latitude;
|
||
var startingPointLng = copter.Longitude;
|
||
|
||
var radius = radiuses[copter];
|
||
|
||
var angVelMin = velMin / radius;
|
||
var angAccel = (clockwise ? 10 : -10) / radius;
|
||
var angVel = 0.0;
|
||
const double DT = INTERVAL / 1000.0;
|
||
var angleTotal = 0.0;
|
||
var startAngle = GeographyUtils.CalcDirection2D(center.Lat, center.Lng, copter.Latitude, copter.Longitude);
|
||
|
||
guide.FinishPredicate = () => angleTotal >= Math.PI * 2;
|
||
|
||
var log = new List<object>();
|
||
|
||
#region guide.UpdateAction
|
||
|
||
guide.UpdateAction = () =>
|
||
{
|
||
if (clockwise)
|
||
{
|
||
if (angVel < angVelMax)
|
||
{
|
||
angVel += angAccel * DT;
|
||
MathUtils.Constrain(ref angVel, 0, angVelMax);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (angVel > angVelMin)
|
||
{
|
||
angVel += angAccel * DT;
|
||
MathUtils.Constrain(ref angVel, angVelMin, 0);
|
||
}
|
||
}
|
||
|
||
var angleChange = angVel * DT;
|
||
angleTotal += angleChange;
|
||
var targetAngle = startAngle + angleTotal;
|
||
var targetLat = center.Lat + Math.Cos(targetAngle) * radius * GeographyUtils.METERS_TO_LAT_SPAN;
|
||
var targetLng = center.Lng + Math.Sin(targetAngle) * radius * GeographyUtils.CalcMetersToLngSpan(copter.Latitude);
|
||
|
||
copter.MoveToIdealState(new CopterState
|
||
{
|
||
Latitude = targetLat,
|
||
Longitude = targetLng,
|
||
Heading = (float)GeographyUtils.RadToDeg(GeographyUtils.CalcDirection2D(targetLat, targetLng, center.Lat, center.Lng)),
|
||
VelocityDirection = GeographyUtils.CalcDirection2D(center.Lat, center.Lng, targetLat, targetLng)
|
||
+ Math.PI / 2,
|
||
Velocity = angVel * radius
|
||
});
|
||
|
||
log.Add(new
|
||
{
|
||
time = DateTime.Now,
|
||
targetLat,
|
||
targetLng,
|
||
lat = copter.Latitude,
|
||
lng = copter.Longitude,
|
||
alt = copter.Altitude,
|
||
angAccel,
|
||
angVel,
|
||
angleChange,
|
||
angleTotal
|
||
});
|
||
};
|
||
|
||
#endregion guide.UpdateAction
|
||
|
||
guide.FinishAction = () =>
|
||
{
|
||
_logger.Log(JsonConvert.SerializeObject(log));
|
||
};
|
||
});
|
||
|
||
await EnsureAllTasksFinishedAsync().ConfigureAwait(false);
|
||
}
|
||
|
||
public Task FlyInRectangleAsync(IEnumerable<ICopter> copters, float startDirection, float sideLength1, float sideLength2, bool clockwise = true, bool loop = false)
|
||
{
|
||
throw new NotImplementedException();
|
||
//await AllStop();
|
||
|
||
//Parallel.ForEach(copters, copter =>
|
||
//{
|
||
// const int pointCount = 4;
|
||
// var points = new LatLng[pointCount];
|
||
// // 把起始点也加进去是为了循环飞行。
|
||
// points[0] = new LatLng { Lat = copter.Latitude, Lng = copter.Longitude };
|
||
|
||
// var directionRad = startDirection * GeographyUtils.DegreesToRad;
|
||
// float distance;
|
||
// LatLng lastPoint;
|
||
|
||
// for (int i = 1; i < pointCount; ++i)
|
||
// {
|
||
// distance = i % 2 != 0 ? sideLength1 : sideLength2;
|
||
// lastPoint = points[i - 1];
|
||
// points[i] = new LatLng
|
||
// {
|
||
// Lat = lastPoint.Lat + distance * Math.Cos(directionRad) * GeographyUtils.MetersToLocalLat,
|
||
// Lng = lastPoint.Lng + distance * Math.Sin(directionRad) * GeographyUtils.GetMetersToLocalLon(lastPoint.Lat)
|
||
// };
|
||
// if (clockwise)
|
||
// {
|
||
// directionRad += Math.PI / 2;
|
||
// }
|
||
// else
|
||
// {
|
||
// directionRad -= Math.PI / 2;
|
||
// }
|
||
// }
|
||
// var target = _guides[copter] ?? new Guide();
|
||
// target.LatLngs = points;
|
||
// target.IsLoop = loop;
|
||
// target.CurrentIndex = 0;
|
||
// _guides[copter] = target;
|
||
|
||
// _logger.Log(string.Join(Environment.NewLine, points.Select(p => $"{p.Lat} {p.Lng}")), Category.Debug, Priority.None);
|
||
//});
|
||
|
||
//await Task.Run(() =>
|
||
//{
|
||
// while (AnyCopterFlyingToLatLng())
|
||
// {
|
||
// Thread.Sleep(100);
|
||
// }
|
||
//});
|
||
}
|
||
|
||
public async Task FlyToVerticalLineAndMakeCircle(IEnumerable<ICopter> copters, float centerDirection, float radius, bool clockwise = true, bool loop = false, double? verLimit = null)
|
||
{
|
||
await AllStop().ConfigureAwait(false);
|
||
|
||
if (!copters.Any()) return;
|
||
|
||
// 1. 飞到不同的高度。
|
||
|
||
int i = 0;
|
||
var tasks = copters.Select(copter =>
|
||
{
|
||
var targetAlt = 40 + 5 * i++;
|
||
return FlyToAltitudeAsync(copter, targetAlt);
|
||
});
|
||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
||
|
||
// 2. 飞到相同经纬度。
|
||
|
||
var targetLat = copters.Average(c => c.Latitude);
|
||
var targetLng = copters.Average(c => c.Longitude);
|
||
tasks = copters.Select(copter =>
|
||
{
|
||
return FlyToLatLngAsync(copter, targetLat, targetLng);
|
||
});
|
||
await Task.WhenAll(tasks).ConfigureAwait(false);
|
||
|
||
// 3. 画圈。
|
||
|
||
await FlyInCircleAsync(copters, centerDirection, radius, clockwise, loop, verLimit);
|
||
}
|
||
}
|
||
|
||
class Guide
|
||
{
|
||
public bool IsLoop { get; set; }
|
||
public Func<bool> FinishPredicate { get; set; }
|
||
public Action FinishAction { get; set; }
|
||
public Action UpdateAction { get; set; }
|
||
}
|
||
|
||
struct CopterState
|
||
{
|
||
public double? Latitude { get; set; }
|
||
public double? Longitude { get; set; }
|
||
public float? Altitude { get; set; }
|
||
public float? Heading { get; set; }
|
||
public double VelocityDirection { get; set; }
|
||
public double Velocity { get; set; }
|
||
}
|
||
|
||
static class ICopterExtensions
|
||
{
|
||
const double DECELERATE_DISTANCE = 5;
|
||
|
||
internal static void MoveToIdealState(this ICopter copter, CopterState idealState)
|
||
{
|
||
ushort channel1, channel2;
|
||
CalcChannel1And2(copter, idealState, out channel1, out channel2);
|
||
|
||
ushort channel3 = CalcChannel3(copter, idealState);
|
||
|
||
//copter.SetChannels(new ChannelBag
|
||
//{
|
||
// Channel1 = channel1,
|
||
// Channel2 = channel2,
|
||
// Channel3 = channel3
|
||
//});
|
||
copter.SetMobileControlAsync(ch1: channel1, ch2: channel2, ch3: channel3, yaw: idealState.Heading);
|
||
}
|
||
|
||
private static void CalcChannel1And2(ICopter copter, CopterState idealState, out ushort channel1, out ushort channel2)
|
||
{
|
||
if (idealState.Latitude == null || idealState.Longitude == null)
|
||
{
|
||
channel1 = channel2 = HOVER_CHANNEL;
|
||
return;
|
||
}
|
||
|
||
var distance = GeographyUtils.CalcDistance2D(copter.Latitude, copter.Longitude, idealState.Latitude.Value, idealState.Longitude.Value);
|
||
|
||
var moveDirection = GeographyUtils.CalcDirection2D(copter.Latitude, copter.Longitude, idealState.Latitude.Value, idealState.Longitude.Value)
|
||
- copter.Heading.DegToRad();
|
||
var coefficient = distance >= DECELERATE_DISTANCE ? 1.0 : distance / DECELERATE_DISTANCE;
|
||
var channelDelta1 = MAX_CHANNEL_DELTA * coefficient;
|
||
var ch1Delta1 = channelDelta1 * Math.Sin(moveDirection);
|
||
var ch2Delta1 = -channelDelta1 * Math.Cos(moveDirection);
|
||
|
||
var channelDelta2 = MAX_CHANNEL_DELTA * idealState.Velocity / MAX_VEL;
|
||
var idealVelDirectionRelativeToHeading = idealState.VelocityDirection - copter.Heading.DegToRad();
|
||
var ch1Delta2 = channelDelta2 * Math.Sin(idealVelDirectionRelativeToHeading);
|
||
var ch2Delta2 = -channelDelta2 * Math.Cos(idealVelDirectionRelativeToHeading);
|
||
|
||
var ch1Delta = ch1Delta1 + ch1Delta2;
|
||
var ch2Delta = ch2Delta1 + ch2Delta2;
|
||
MathUtils.Constrain(ref ch1Delta, -MAX_CHANNEL_DELTA, MAX_CHANNEL_DELTA);
|
||
MathUtils.Constrain(ref ch2Delta, -MAX_CHANNEL_DELTA, MAX_CHANNEL_DELTA);
|
||
|
||
channel1 = (ushort)(ch1Delta > 0 ? MAX_HOVER_CHANNEL + ch1Delta : MIN_HOVER_CHANNEL + ch1Delta);
|
||
channel2 = (ushort)(ch2Delta > 0 ? MAX_HOVER_CHANNEL + ch2Delta : MIN_HOVER_CHANNEL + ch2Delta);
|
||
}
|
||
|
||
private static ushort CalcChannel3(ICopter copter, CopterState idealState)
|
||
{
|
||
if (idealState.Altitude == null)
|
||
{
|
||
return HOVER_CHANNEL;
|
||
}
|
||
|
||
var distance = idealState.Altitude - copter.Altitude;
|
||
var coefficient = distance >= DECELERATE_DISTANCE ? 1.0 : distance / DECELERATE_DISTANCE;
|
||
var ch3Delta = MAX_CHANNEL_DELTA * coefficient;
|
||
return (ushort)(ch3Delta > 0 ? MAX_HOVER_CHANNEL + ch3Delta : MIN_HOVER_CHANNEL + ch3Delta);
|
||
}
|
||
}
|
||
}
|