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