コード例 #1
0
 public static Angles ToDegrees(Angles r)
 {
     var degrees = new Angles
                       {
                           BearingAngle = MathHelper.ToDegrees(r.BearingAngle),
                           BearingErrorAngle = MathHelper.ToDegrees(r.BearingErrorAngle),
                           GoalAngle = MathHelper.ToDegrees(r.GoalAngle),
                           HeadingAngle = MathHelper.ToDegrees(r.HeadingAngle),
                           PitchAngle = MathHelper.ToDegrees(r.PitchAngle),
                           RollAngle = MathHelper.ToDegrees(r.RollAngle)
                       };
     return degrees;
 }
コード例 #2
0
        /// <summary>
        /// TODO Temporary. We might later distinguish between physical state and navigation state.
        /// </summary>
        /// <param name="s"></param>
        /// <param name="heightAboveGround"></param>
        /// <param name="waypoint"></param>
        /// <param name="output"></param>
        /// <returns></returns>
        public static HeliState ToHeliState(PhysicalHeliState s, float heightAboveGround, Waypoint waypoint, JoystickOutput output)
        {
            // Update state
            var r = new HeliState
                        {
                            HeightAboveGround = heightAboveGround,
                            Orientation = s.Orientation,
                            Forward = s.Axes.Forward,
                            Up = s.Axes.Up,
                            Right = s.Axes.Right,
                            Output = output,
                            Position = s.Position,
                            Velocity = s.Velocity,
                            Acceleration = s.Acceleration,
                            Waypoint = waypoint,
                            HPositionToGoal = VectorHelper.ToHorizontal(waypoint.Position - s.Position),
                        };

            // Update angles from current state
            var radians = new Angles
                              {
                                  PitchAngle = VectorHelper.GetPitchAngle(s.Orientation),
                                  RollAngle = VectorHelper.GetRollAngle(s.Orientation),
                                  HeadingAngle = VectorHelper.GetHeadingAngle(s.Orientation),
                              };

            // Velocity can be zero, and thus have no direction (NaN angle)
            radians.BearingAngle = (s.Velocity.Length() < 0.001f)
                                       ? radians.HeadingAngle
                                       : VectorHelper.GetHeadingAngle(s.Velocity);
                //GetAngle(VectorHelper.ToHorizontal(s.Velocity));

            radians.GoalAngle = VectorHelper.GetAngle(r.HPositionToGoal);
            radians.BearingErrorAngle = VectorHelper.DiffAngle(radians.BearingAngle, radians.GoalAngle);

            // Store angles as both radians and degrees for ease-of-use later
            r.Radians = radians;
            r.Degrees = ToDegrees(radians);
            return r;
        }