Retrograde Locking

[UranianBlue]

I started working on a new Module that will allow me to do a hands-off powered descent and mun lander.

The module is good at controlling the throttle. However, it is not good at keeping the craft locked on its retrograde!

At first, the ship will point almost exactly at the retrograde. However, after several seconds, the craft will stop following the retrograde, and only adjust once the retrograde has drifted very far from the craft's heading. Using SAS, I can click on the hold retrograde icon, and it fixes the problem. This works, but I don't want people to have to mess with their SAS.

Here is my code, and I also have a video of a working attempt:

Spoiler

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using UnityEngine;

namespace UBPPVacMoon
{
    /// Will auto pilot a given craft
    /// UBPP AutoPilot Segway tm
    /// a WIP
    public class LunarAutoPilot : PartModule
    {
        
        [KSPField(isPersistant = true, guiActive = true, guiActiveEditor = true, guiName = "Pilot"),
            UI_Toggle(enabledText = "On", disabledText = "Off")]
        public bool landerEnabled = false;
        
        [KSPField(isPersistant = true, guiActive = true, guiActiveEditor = true, guiName = "Activation Altitude"),
            UI_FloatRange(minValue = 150.0F, maxValue = 12000.0F, stepIncrement = 50.0F, scene = UI_Scene.All)]
        public float actAltitude = 5000.0F;
        
        void Start()
        {
            if (landerEnabled)
            {
                ActivatePilot();
            }
            
            RefreshPartWindow();
        }
        
        [KSPAction("Activate Pilot")]
        public void AGActivatePilot(KSPActionParam param)
        {
            ActivatePilot();
        }
        
        [KSPAction("Deactivate Pilot")]
        public void AGDeactivatePilot(KSPActionParam param)
        {
            DeactivatePilot();
        }
        
        [KSPAction("Toggle Pilot")]
        public void AGTogglePilot(KSPActionParam param)
        {
            TogglePilot();
        }
        
        public void ActivatePilot()
        {
            Vector3d position = vessel.vesselTransform.position;
            double altitude = vessel.mainBody.GetAltitude(position);
            
            if (altitude < actAltitude)
            {
                if (landerEnabled != true)
                {
                    landerEnabled = true;
                    vessel.OnFlyByWire += new FlightInputCallback(AutoPilot);
                }
            }
            
            RefreshPartWindow();
        }
        
        //AutoPilot is the method which pilots the ship!
        
        public void DeactivatePilot()
        {
            if (landerEnabled)
            {
                landerEnabled = false;
                vessel.OnFlyByWire -= AutoPilot;
            
                RefreshPartWindow();
            }
        }
        
        public void TogglePilot()
        {
            if (landerEnabled)
            {
                DeactivatePilot();
            }
            else
            {
                ActivatePilot();
            }
        }

        void onPartDestroy()
        {
            vessel.OnFlyByWire -= AutoPilot;
        }
        
        void onDisconnect()
        {
            vessel.OnFlyByWire -= AutoPilot;
        }
        
        void RefreshPartWindow()
        {
            Events["TogglePilot"].guiName = landerEnabled ? "Deactivate Pilot" : "Activate Pilot";

            //Misc.RefreshAssociatedWindows(part);
        }
        
        void AutoPilot(FlightCtrlState s)
        {
            if (landerEnabled)
            {
                Vector3d position = vessel.vesselTransform.position;
            
                Vector3d eastUnit = vessel.mainBody.getRFrmVel(position).normalized;
                Vector3d upUnit = (position - vessel.mainBody.position).normalized;
                Vector3d northUnit = Vector3d.Cross(upUnit, eastUnit);
            
                float altitude = vessel.GetHeightFromTerrain();
            
                Vector3d rotatingFrameVelocity = vessel.srf_velocity;
            
                Vector3d geeAcceleration = FlightGlobals.getGeeForceAtPosition(position);
            
                Vector3d heading = (Vector3d)vessel.transform.up;
            
                // now, based on what's happening, control the ship!!
                // first, we want the ship to point in the retrograde vector
                // seccond, our target speed needs to be equal to our surface altitude divided by 10s
                // that's it!!!
            
                Vector3d dir = -1.0 * rotatingFrameVelocity;
                Vector3d error = (Vector3d)vessel.transform.InverseTransformDirection((Vector3d)dir);
                double x = (double)error.x;
                double z = (double)error.z;
                double k = 0.0500F * ((double)0.0250F - (double)Mathf.Sqrt((float)x * (float)x + (float)z * (float)z)) * (double)500.0F;
                if (k <= (double)0.0F)
                {
                    k = (double)0.0500F;
                }
                double yaw_command = k * x;
                double pitch_command = -k * z;
                
                if (yaw_command < (double)0.008)
                {
                    yaw_command *= (double)2.0;
                }
                
                if (pitch_command < (double)0.008)
                {
                    pitch_command *= (double)2.0F;
                }
                
                s.roll = 0.0F;
            
                s.yaw += (float)yaw_command * 1.0F;
                s.pitch += (float)pitch_command * 1.0F;
                s.yaw = Mathf.Clamp(s.yaw, -1.57F, 1.57F);
                s.pitch = Mathf.Clamp(s.pitch, -1.57F, 1.57F);
            
                float tgtVelocity = 3.5F + (((float)altitude + 20.0F) / 6.5F);
                float curVelocity = (float)Mathf.Sqrt((float)(rotatingFrameVelocity.x * rotatingFrameVelocity.x) + (float)(rotatingFrameVelocity.y * rotatingFrameVelocity.y) + (float)(rotatingFrameVelocity.z * rotatingFrameVelocity.z));
            
                if (true)  // we better throttle up now!!!
                {
                    s.mainThrottle += (curVelocity - tgtVelocity) / curVelocity * 1.0F;
                
                    s.mainThrottle = Mathf.Clamp(s.mainThrottle, 0.0F, +10000.0F);
                }
            }
        }
    }
}

 

[Katten]

You'd need a PI or PID regulator! EDIT: You already do I think in 's'! lol. I'm new to ksp programming

Edited October 10, 2017 by Katten