the control algorithm in autopilot may not converge
BlackBreezeCastle opened this issue ยท 1 comments
I found that the autopilot do not converge sometimes,Then I checked the codes and found that it just simplely count the difference of quaternion between the target rotation and the current rotation as the rotation to do which may cause attitude control diverge.So I modified a function in the file AttitudeController.cs to solve the problem and it works well
code is there
Vector3 ComputeTargetAngularVelocity(Vector3d torque, Vector3d moi)
{
var internalVessel = vessel.InternalVessel;
var currentRotation = ReferenceFrame.RotationFromWorldSpace(internalVessel.ReferenceTransform.rotation);
var currentDirection = ReferenceFrame.DirectionFromWorldSpace(internalVessel.ReferenceTransform.up);
QuaternionD rotation = Quaternion.FromToRotation(currentDirection, targetDirection);
// Compute angles for the rotation in pitch (x), roll (y), yaw (z) axes
float angleFloat;
Vector3 axisFloat;
// FIXME: QuaternionD.ToAngleAxis method not available at runtime
((Quaternion)rotation).ToAngleAxis(out angleFloat, out axisFloat);
double angle = GeometryExtensions.ClampAngle180(angleFloat);
Vector3d axis = axisFloat;
var angles = axis * angle;
if (!double.IsNaN(TargetRoll))
{
// Roll angle set => use rotation from currentRotation -> targetRotation
rotation = rotation.Inverse()* targetRotation;
rotation = rotation*currentRotation.Inverse();
((Quaternion)rotation).ToAngleAxis(out angleFloat, out axisFloat);
if (!float.IsInfinity(axisFloat.magnitude))
{
angle = GeometryExtensions.ClampAngle180(angleFloat);
axis = axisFloat;
angles = angles + axis * angle;
}
}
// else Roll angle not set => use rotation from currentDirection -> targetDirection
// FIXME: QuaternionD.FromToRotation method not available at runtime
angles = vessel.ReferenceFrame.DirectionFromWorldSpace(ReferenceFrame.DirectionToWorldSpace(angles));
return AnglesToAngularVelocity (angles, torque, moi);
}
