public AngularVelocity3 Manipulate(AngularVelocity3 processVariable, AngularVelocity3 setpoint, double elapsedTime) { double manipulatedVariableX = _controllerX.Manipulate(processVariable.X, setpoint.X, elapsedTime); double manipulatedVariableY = _controllerX.Manipulate(processVariable.Y, setpoint.Y, elapsedTime); double manipulatedVariableZ = _controllerX.Manipulate(processVariable.Z, setpoint.Z, elapsedTime); return(new AngularVelocity3(manipulatedVariableX, manipulatedVariableY, manipulatedVariableZ)); }
public static IObservable <Quaternion> ToRotation(this IObservable <AngularVelocity3> source) { return(source .TimeInterval() .Select(angularVelocityTimed => { AngularVelocity3 angularVelocity = angularVelocityTimed.Value; double dt = angularVelocityTimed.Interval.TotalSeconds; Vector3 w = angularVelocity.Normalize(); double theta = AngleConvert.ToRadians(angularVelocity.Magnitude) * dt; return Quaternion.FromAxisAngle(w, -theta); })); }
private void Initialize() { Thrust.Subscribe(value => _latestThrust = value); AngularVelocityControl.Subscribe(value => { var scale = new AngularVelocity3( x: 720d /*dps*/, y: 720d /*dps*/, z: 720d /*dps*/); _latestPitch = Math.Min(Math.Max(value.X / scale.X, -1d), 1d); _latestRoll = Math.Min(Math.Max(value.Y / scale.Y, -1d), 1d); _latestYaw = Math.Min(Math.Max(value.Z / scale.Z, -1d), 1d); }); }
public static IObservable <AngularAcceleration3> Differentiate(this IObservable <AngularVelocity3> source) { return(source .Scan( new { Prior = default(AngularVelocity3), Current = default(AngularVelocity3) }, (accu, angularVelocity) => new { Prior = accu.Current, Current = angularVelocity }) .Skip(1) .TimeInterval() .Select(t => { AngularVelocity3 dw = t.Value.Current - t.Value.Prior; double dt = t.Interval.TotalSeconds; return new AngularAcceleration3( x: dw.X / dt, y: dw.Y / dt, z: dw.Z / dt); })); }