public bool Update(GCS gcs, float rotation, out int roll, out int pitch)
{
try
{
if (this.IsFlying == false)
{
throw new Exception();
}
if (gcs.IsValid == false)
{
throw new Exception();
}
this._currentGCS = gcs;
this._rotation = rotation;
//this._rotation -= (float)(-6.7f * (Math.PI / 180.0f));
//this._rotation -= (float)(-6.7f * (Math.PI / 180.0f));
// 현재 드론의 회전 각도
var degree = this._rotation * 180 / Math.PI;
// 드론의 현재 위치로부터 목표지점까지의 벡터
var vector = OYOGmap.GetVector(this._currentGCS, this.Destination);
// 드론의 회전정도만큼 벡터를 회전
vector.x = Math.Cos(this._rotation) * vector.x - Math.Sin(this._rotation) * vector.y;
vector.y = Math.Sin(this._rotation) * vector.x + Math.Cos(this._rotation) * vector.y;
var normal = vector.Normalized;
var distance = vector.Magnitude;
Console.WriteLine("distance : {0}m", (distance * 1000.0f).ToString("0.00"));
if (distance * 1000.0f < 1)
{
this._currentDestinationIndex++;
roll = pitch = 0;
this.OnLookNextDestination?.Invoke();
Console.WriteLine("next point");
if (this._currentDestinationIndex > this._destinations.Length - 1)
{
this.IsFlying = false;
this.OnComplete?.Invoke();
Console.WriteLine("complete");
}
return(true);
}
// 이게 무조건 양수인 문제가 있음
var maxSpeed = (int)Math.Max(1, MAX_SPEED * Math.Min(1, (distance - MIN_DISTANCE) / (MAX_DISTANCE - MIN_DISTANCE)));
var directionX = normal.x > 0 ? 1 : -1;
var directionY = normal.y > 0 ? 1 : -1;
if (Math.Abs(normal.x / normal.y) > 0) // x가 y보다 클 때
{
roll = maxSpeed * directionX;
pitch = (int)(maxSpeed * Math.Abs(normal.y / normal.x)) * directionY;
}
else
{
pitch = maxSpeed * directionX;
roll = (int)(maxSpeed * Math.Abs(normal.x / normal.y)) * directionY;
}
//Console.WriteLine("x speed : {0}, z speed : {1}", roll, pitch);
//if (this.IsFlying == false)
//{
// pitch = roll = 0;
//}
return(true);
}
catch (Exception)
{
roll = pitch = 0;
return(false);
}
}
public static double GetDistance(GCS begin, GCS end)
{
return(GetDistance(begin.lat, begin.lon, end.lat, end.lon));
}
public void SetPosition(GCS coord, bool update = false)
{
this.SetPosition(coord.lat, coord.lon, update);
}
public static OYOVector GetVector(GCS gcs1, GCS gcs2)
{
return(GetVector(gcs1.lat, gcs1.lon, gcs2.lat, gcs2.lon));
}
public void RequestAddress(string name, GCS gcs)
{
var requestAddrThread = new Thread(new ParameterizedThreadStart(this.RequestAddressThreadRoutine));
requestAddrThread.Start(new object[] { name, gcs });
}
