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