Exemplo n.º 1
0
        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);
            }
        }
Exemplo n.º 2
0
 public static double GetDistance(GCS begin, GCS end)
 {
     return(GetDistance(begin.lat, begin.lon, end.lat, end.lon));
 }
Exemplo n.º 3
0
 public void SetPosition(GCS coord, bool update = false)
 {
     this.SetPosition(coord.lat, coord.lon, update);
 }
Exemplo n.º 4
0
 public static OYOVector GetVector(GCS gcs1, GCS gcs2)
 {
     return(GetVector(gcs1.lat, gcs1.lon, gcs2.lat, gcs2.lon));
 }
Exemplo n.º 5
0
        public void RequestAddress(string name, GCS gcs)
        {
            var requestAddrThread = new Thread(new ParameterizedThreadStart(this.RequestAddressThreadRoutine));

            requestAddrThread.Start(new object[] { name, gcs });
        }