public void UpdateThrustControl(MatrixD orientation, Vector3D linearVelocityWorld)
{
Vector3D velocity = Vector3D.TransformNormal(linearVelocityWorld, MatrixD.Transpose(orientation));
Vector3D destinationRelative = Vector3D.TransformNormal(destination - orientation.Translation, MatrixD.Transpose(orientation));
float mass = navigation.Mass;
Vector3D availableBrakingAcceleration = navigation.MaxThrustInDirection(-velocity) / mass;
Vector3D predictedStopPoint = -0.5 * velocity * velocity / availableBrakingAcceleration;
Vector3D positionError = destinationRelative - predictedStopPoint;
Vector3D desiredVelocity = Vector3D.ClampToSphere(thrustPID.Filter(positionError), SpeedCap);
Vector3D velocityError = velocity - desiredVelocity;
Vector3D desiredThrustPercentage = Vector3D.Clamp(0.5 * velocityError, -Vector3D.One, Vector3D.One);
thrustDebug?.Reset();
thrustDebug?.WriteLine($"x = {DebugPanel.ToString(destinationRelative)}");
thrustDebug?.WriteLine($"v = {DebugPanel.ToString(velocity)}");
thrustDebug?.WriteLine($"as = {DebugPanel.ToString(availableBrakingAcceleration)}");
thrustDebug?.WriteLine($"xs = {DebugPanel.ToString(predictedStopPoint)}");
thrustDebug?.WriteLine($"vt = {DebugPanel.ToString(desiredVelocity)}");
thrustDebug?.WriteLine($"v_err = {DebugPanel.ToString(velocityError)}");
thrustDebug?.WriteLine($"t = {DebugPanel.ToString(desiredThrustPercentage)}");
navigation.SetThrustPercentage(desiredThrustPercentage);
}
public void UpdateGyroControl(MatrixD orientation, Vector3D angularVelocityWorld)
{
// X - pitch
// Y - yaw
// Z - roll
headingDebug?.Reset();
if (Track == null)
{
headingDebug?.WriteLine("No track.");
return;
}
Vector3D trackDirection = Track.Forward;
headingDebug?.WriteLine($"d = {Math.Round(trackDirection.X, 2)}, {Math.Round(trackDirection.Y, 2)}, {Math.Round(trackDirection.Z, 2)}");
Vector3D angularVelocity = Vector3D.TransformNormal(angularVelocityWorld, MatrixD.Transpose(orientation));
headingDebug?.WriteLine($"w = {Math.Round(angularVelocity.X, 2)}, {Math.Round(angularVelocity.Y, 2)}, {Math.Round(angularVelocity.Z, 2)}");
Vector3D angularVelocityVectorDirTarget = Vector3D.Cross(Vector3D.Forward, trackDirection);
headingDebug?.WriteLine($"wt = {Math.Round(angularVelocityVectorDirTarget.X, 2)}, {Math.Round(angularVelocityVectorDirTarget.Y, 2)}, {Math.Round(angularVelocityVectorDirTarget.Z, 2)}");
double rotationError = Math.Acos(trackDirection.Z);
const double maxAlpha = .5;
double approachOmega = -Vector3D.Dot(angularVelocity, angularVelocityVectorDirTarget);
headingDebug?.WriteLine($"w_app = {Math.Round(approachOmega, 3)}");
double stoppingTheta = approachOmega / (2.0 * maxAlpha);
double rotationErrorStopping = rotationError - stoppingTheta;
headingDebug?.WriteLine($"e = {Math.Round(rotationError * 180.0 / Math.PI,2)}, ts = {Math.Round(stoppingTheta * 180.0 / Math.PI, 2)}, es = {Math.Round(rotationErrorStopping * 180.0 / Math.PI, 2)}");
// We have to force the angular velocity vector to always be in the upper half-plane, so that the rotationError signal
// is bidirectional. If the rotationError signal isn't bidirectional then we can't unwind the integral component of
// the PID controller and tracking breaks.
if (angularVelocityVectorDirTarget.Y < 0)
{
angularVelocityVectorDirTarget = -angularVelocityVectorDirTarget;
rotationErrorStopping = -rotationErrorStopping;
}
bool useIntegral = rotationError < rotationIntegralBreakaway;
double angularVelocityVectorMagnitudeTarget = rotationPID.Filter(rotationErrorStopping, !useIntegral);
headingDebug?.WriteLine($"pid_i = {rotationPID.IntegralState}");
Vector3D angularVelocityTarget = Vector3D.ClampToSphere(-angularVelocityVectorMagnitudeTarget * angularVelocityVectorDirTarget, Math.PI);
Vector3D angularVelocityError = angularVelocityTarget - angularVelocity;
// I'm not adding any extra control signal to angular velocity. I've experimented to try eke out a little more torque by overcompensating but the increased settling time
// doesn't make it worthwhile. Maybe on a different drone design.
Vector3D angularControl = angularVelocityTarget;
headingDebug?.WriteLine($"control = {Math.Round(angularControl.X, 3)}, {Math.Round(angularControl.Y, 3)}");
angularControl = Vector3D.ClampToSphere(angularControl, Math.PI);
ApplyGyroControlUpdate(angularControl, 0.0);
}
