private bool IsMove(EulerAngleSpeed speed)
{
if (Math.Abs(speed.PitchSpeed) < _minSpeed && Math.Abs(speed.YawSpeed) < _minSpeed && Math.Abs(speed.RollSpeed) < _minSpeed)
{
return(false);
}
else
{
return(true);
}
}
/// <summary>
/// 根据速度计算角度偏移量
/// </summary>
/// <param name="speed">传感器速度监测值</param>
/// <returns></returns>
public EulerAngle EulerAngleOffset(EulerAngleSpeed speed)
{
EulerAngle angle = new EulerAngle();
if (IsMove(speed))
{
angle.Pitch = speed.PitchSpeed * _speedK;
angle.Yaw = speed.YawSpeed * _speedK;
angle.Roll = speed.RollSpeed * _speedK;
}
return(angle);
}
private EulerAngleSpeed RangeSpeed(EulerAngleSpeed angleSpeed, double maxSpeed)
{
if (Math.Abs(angleSpeed.PitchSpeed) > maxSpeed)
{
angleSpeed.PitchSpeed = angleSpeed.PitchSpeed / Math.Abs(angleSpeed.PitchSpeed) * maxSpeed;
}
if (Math.Abs(angleSpeed.YawSpeed) > maxSpeed)
{
angleSpeed.YawSpeed = angleSpeed.YawSpeed / Math.Abs(angleSpeed.YawSpeed) * maxSpeed;
}
if (Math.Abs(angleSpeed.RollSpeed) > maxSpeed)
{
angleSpeed.RollSpeed = angleSpeed.RollSpeed / Math.Abs(angleSpeed.RollSpeed) * maxSpeed;
}
return(angleSpeed);
}
private void OnUpperArmUpdate(EulerAngleSpeed angleSpeed)
{
//_latestUpperArmAngleSpeeds = angles;
angleSpeed = RangeSpeed(angleSpeed, _maxSpeed);
_latestUpperArmAngleSpeed = AverageAnglesSpeed(_latestUpperArmAngleSpeed, angleSpeed);
}
private EulerAngleSpeed AverageAnglesSpeed(EulerAngleSpeed speed1, EulerAngleSpeed speed2)
{
return(new EulerAngleSpeed {
PitchSpeed = (speed1.PitchSpeed + speed2.PitchSpeed) / 2, YawSpeed = (speed1.YawSpeed + speed2.YawSpeed) / 2, RollSpeed = (speed1.RollSpeed + speed2.RollSpeed) / 2
});
}
