Автопилот для ховеркрафтов

using System;
using System.Threading;
using KRPC.Client;
using KRPC.Client.Services.SpaceCenter;
using KRPC.Client.Services.UI;
namespace KSP_Howercraft
{
    class Program
    {
        //Высота по умолчанию 5 метров.
        private static double trgalt = 5;
        private static double trgspd;
        private static InputField input;
        private static Button btn;
        private static Button mode;
        //Режим удержания высоты (над поверхностью/над уровнем моря)
        private static bool IsSrf = true;
        static void Main(string[] args)
        {
            var con = new Connection();
            var sc = con.SpaceCenter();
            var vessel = sc.ActiveVessel;
            var srfalt = con.AddStream(() => vessel.Flight(vessel.Orbit.Body.ReferenceFrame).SurfaceAltitude);
            var vspd = con.AddStream(() => vessel.Flight(vessel.Orbit.Body.ReferenceFrame).VerticalSpeed);
            var ui = con.UI();
            Canvas c = ui.AddCanvas();
            c.Visible = true;
            input = c.AddInputField();
            btn = c.AddButton("Применить");
            mode = c.AddButton("Режим");
            input.RectTransform.Position = new Tuple<double, double>(0, 260);
            btn.RectTransform.Position = new Tuple<double, double>(0, 230);
            mode.RectTransform.Position = new Tuple<double, double>(170, 230);
            //Так как я хочу добавить больше функционала, удержание высоты засунул в отдельный поток.
            Action <Vessel, Stream<double>, Stream<double>> keepalt = KeepAlt;
            keepalt.BeginInvoke(vessel, vspd, srfalt, null, null);
            Console.ReadKey();
        }
        static void KeepAlt(Vessel vessel, Stream<double> vspd, Stream<double> srfalt)
        {
            while (true)
            {
                var maxthrust = vessel.AvailableThrust;
                //Обрабатываем нажатие кнопки ввода высоты
                if (btn.Clicked)
                {
                    btn.Clicked = false;
                    trgalt = Convert.ToDouble(input.Value.Replace('.', ','));
                }
                var p = vessel.Position(vessel.Orbit.Body.ReferenceFrame);
                //Находим расстояние до центра планеты.
                var alt = Math.Sqrt(Math.Sqrt(p.Item1 * p.Item1 + p.Item2 * p.Item2) + p.Item3 * p.Item3);
                //Находим горизонтальную орбитальную скорость.
                var hororbspd = Math.Sqrt(vessel.Orbit.Speed*vessel.Orbit.Speed - vspd.Get()*vspd.Get());
                //Гравитация           Гравитация на данной высоте                    Центробежное ускорение
                var gravity = vessel.Orbit.Body.GravitationalParameter/(alt*alt)- hororbspd * hororbspd / alt;
                //Вес
                float weight = (float)(vessel.Mass*gravity);
                //Тяга, уравновешивающая силу тяжести
                float defthr = weight/maxthrust;
                //Нажатие кнопки изменения режима.
                if (mode.Clicked)
                {
                    mode.Clicked = false;
                    IsSrf ^= true;
                    if (IsSrf)
                    {
                        trgalt += srfalt.Get() - vessel.Flight(vessel.Orbit.Body.ReferenceFrame).MeanAltitude;
                    }
                    else
                    {
                        trgalt -= srfalt.Get() - vessel.Flight(vessel.Orbit.Body.ReferenceFrame).MeanAltitude;
                    }
                    input.Value = trgalt.ToString();
                }
                //Находим разницу высот
                double altdiff;
                if (IsSrf)
                {
                    altdiff = trgalt - srfalt.Get();
                }
                else
                {
                    altdiff = trgalt - vessel.Flight(vessel.Orbit.Body.ReferenceFrame).MeanAltitude;
                }
                if (altdiff > 0)
                {
                    //Нужная вертикальная скорость в зависимости от гравитации.
                    trgspd = gravity * Math.Sqrt(altdiff / 2 / gravity);
                }
                else
                {
                    //При спуске выбираю вертикальную скорость на свой вкус.
                    trgspd = altdiff / 2;
                }
                //Угол наклона аппарата.
                double ang = vessel.Flight(vessel.SurfaceReferenceFrame).Direction.Item1;
                //Устанавливаем тягу.
                vessel.Control.Throttle = Cut((defthr + (float)(CutSpeed((float)trgspd) - vspd.Get()) * vessel.Mass / maxthrust)/(float)ang);
                Thread.Sleep(50);
            }
        }
        //Не даем вертикальной скорости опуститься ниже -20 м/с
        static float CutSpeed(float x)
        {
            if (x < -20) return -20;
            return x;
        }
        //Ограничиваем тягу между 0 и 1 
        static float Cut(float x)
        {
            if (x > 1) return 1;
            if (x < 0) return 0;
            return x;
        }
    }
}