Setting Autopilot.SAS Hold Orientation

[Kramer]

I am working on a plugin for docking and am trying to align the current vessel with the docking axis of the target. I am trying to use the vessel Autopilot.SAS to do this and so far it does not seem to work.

In my FixedUpdate() handler I am first ensuring that it is in StabilityAssist mode:
 

    if (vessel.Autopilot.Mode != VesselAutopilot.AutopilotMode.StabilityAssist)
            {
                Deb.Log("Engaging StabilityAssist");
                vessel.Autopilot.SetMode(VesselAutopilot.AutopilotMode.StabilityAssist);
                sas_orientation_set = false;
            }

And then calculating a quaternion based on the target zaxis (which is the target transform forward vector) and the target transform up vector:

            Quaternion quat = Quaternion.LookRotation(-zaxis, tgt_tr.up.normalized);

And then setting that if I have not already set it:

            if (!sas_orientation_set)
            {
                Deb.Log("SAS set to quat: {0}", quat);
                vessel.Autopilot.SAS.LockRotation(quat);
                sas_orientation_set = true;
            }

I also am printing out the current "lockedRotation" quant and what I set it to:

            Deb.Log("currentRotationLock: {0}", vessel.Autopilot.SAS.lockedRotation);
            Deb.Log("desiredRotationLock: {0}", quat);

Initially the two values match up, but after a few updates, the lockedRotation value changes to what it was before I set it:

[LOG 23:42:22.139] [KRAMAX] currentRotationLock: (0.0, -0.6, 0.0, -0.8)
[LOG 23:42:22.139] [KRAMAX] desiredRotationLock: (-0.3, -0.3, 0.2, 0.9)
[LOG 23:42:22.140] [KRAMAX] SAS set to quat: (-0.3, -0.3, 0.2, 0.9)
[LOG 23:42:22.163] [KRAMAX] currentRotationLock: (-0.3, -0.3, 0.2, 0.9)
[LOG 23:42:22.164] [KRAMAX] desiredRotationLock: (-0.3, -0.3, 0.2, 0.9)
[LOG 23:42:22.164] [KRAMAX] SAS set to quat: (-0.3, -0.3, 0.2, 0.9)
[LOG 23:42:22.187] [KRAMAX] currentRotationLock: (-0.3, -0.3, 0.2, 0.9)
[LOG 23:42:22.187] [KRAMAX] desiredRotationLock: (-0.3, -0.3, 0.2, 0.9)
[LOG 23:42:22.192] [KRAMAX] currentRotationLock: (0.0, -0.6, 0.0, -0.8)
[LOG 23:42:22.193] [KRAMAX] desiredRotationLock: (-0.3, -0.3, 0.2, 0.9)
[LOG 23:42:22.215] [KRAMAX] currentRotationLock: (0.0, -0.6, 0.0, -0.8)
 

Does anyone know how to make this work? I would really like to use SAS to do this and not have to write my own yaw/pitch/roll autopilot or require users to use mechjeb.

-Thanks, Kramer

[Kramer]

Well I figured this out, so to answer my own question...

I think I read in another post that you cannot change the rotation lock my a large amount. Thinking about how it is used via the GUI this makes some sense. It normally keeps the __CURRENT__ orientation when you engage this mode. So I changed the code to gradually shift the rotation lock quaternion from the current position to the desired one. This worked very well.

Here is code that I came up with that I put in my FixedUpdate() handler:

            bool sasWasOn = vessel.ActionGroups[KSPActionGroup.SAS];

            vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
            vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true);

            if (vessel.Autopilot.Mode != VesselAutopilot.AutopilotMode.StabilityAssist || !vessel.Autopilot.Enabled || !sasWasOn)
            {
                vessel.Autopilot.Enable(VesselAutopilot.AutopilotMode.StabilityAssist);
                sas_orientation_set = false;
            }

            Quaternion quat = Quaternion.LookRotation(tgt_tr.up.normalized, -zaxis);
            Quaternion cquat = vessel.Autopilot.SAS.lockedRotation;

            float angle = Math.Abs(Quaternion.Angle(cquat, quat));

         
            // really we just want to move like 0.1 degrees per update or something
            // so to do that we would have fractional amount f: f*angle = 0.1 or f = 0.1/angle
            if (angle > 0)
            {
                float f = 0.1f / angle;
                Quaternion getting_there = Quaternion.Slerp(cquat, quat, f);

                vessel.Autopilot.SAS.LockRotation(getting_there);
                sas_orientation_set = true;
            }
            else if (!sas_orientation_set)
            {
                vessel.Autopilot.SAS.LockRotation(quat);
                sas_orientation_set = true;
            }
 

Hope this may help anyone trying to do the same thing.