Example #1
0
 public void setGPS(maths.Vector3 pos)
 {
     _pos = pos;
     return;
 }
        private bool GoToCheckpoint()
        {
            bool checkpointsDone = false;
            if (_actualCheckpoint <= _targetCheckpoints.Count - 1)
            {
                _targetCheckpoints[_actualCheckpoint].VisualPoint.IsVisible = true;
                maths.Vector3 errorVector = _targetCheckpoints[_actualCheckpoint].Point - base.Drone.GPS;

                //Altitude target
                base.TargetAltitude = _targetCheckpoints[_actualCheckpoint].Point.Y;

                //Yaw target
                float distance = errorVector.Length();
                maths.Vector3 direction = errorVector;
                direction.Normalize();
                if (distance > 0.5f)
                {
                    float errorYaw = (float)Math.Atan2(direction.X, -direction.Z) * 180 / (float)Math.PI;
                    base.TargetYaw = errorYaw;
                }

                //Pitch target

                maths.Vector3 cosPitchAxis = new maths.Vector3(base.Drone.Pose.M13, base.Drone.Pose.M23, base.Drone.Pose.M33);
                float errorPitch = -maths.Vector3.Dot(errorVector, cosPitchAxis);

                if (Math.Abs(errorPitch) > DistanceTargetPitch)
                {
                    errorPitch = DistanceTargetPitch * Math.Sign(errorPitch);
                }

                base.TargetPitch = errorPitch * MaxTargetPitch / (float)DistanceTargetPitch;

                //Roll target
                maths.Vector3 sinRollAxis = new maths.Vector3(base.Drone.Pose.M11, base.Drone.Pose.M21, base.Drone.Pose.M31);
                float errorRoll = -maths.Vector3.Dot(errorVector, sinRollAxis);

                if (Math.Abs(errorRoll) > DistanceTargetRoll)
                {
                    errorRoll = DistanceTargetRoll * Math.Sign(errorRoll);
                }

                base.TargetRoll = errorRoll * MaxTargetRoll / (float)DistanceTargetRoll;

                if (distance < _targetCheckpoints[_actualCheckpoint].DistanceError)
                {
                    _targetCheckpoints[_actualCheckpoint].VisualPoint.IsVisible = false;
                    if (_actualCheckpoint == _targetCheckpoints.Count - 1)
                    {
                        checkpointsDone = true;
                    }
                    _actualCheckpoint++;
                }
            }
            return checkpointsDone;
        }
Example #3
0
 private void BindJoint(Object sender, EventArgs<IJoint> args)
 {
     _joint = (IMotorizedHingeJoint)args.Item;
     _axis = ((MotorizedHingeJointDesc)_joint.Descriptor).Axis;
     TargetRPM = 0;
 }