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;
}
/// <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;
}