【不务正业】太空工程师自动导航v1.0 beta

//By dudujerry
//2021.4.17
//自动导航 v1.0 beta

?

Vector3D TARGET = new Vector3D(158267.95,-25453.61,-60794.93);

 

List<IMyThrust> _thrusts = new List<IMyThrust>();

List<IMyShipController> _shipControls = new List<IMyShipController>();

List<IMyCameraBlock> _camera = new List<IMyCameraBlock>();

List<IMyGyro> _gyros = new List<IMyGyro>();

IMyShipController _cockpit;

 

Vector3D _MaxAngleAcceleration;

Vector3D _InitAngleVelocity;

Vector3D targetThrustPower = new Vector3D();

Vector3D t = new Vector3D();

 

bool _dam;

int debug = 0;

double debug2 = 0;

int _state;//0代表正在寻找位置,1代表降落

double _maxThrust;

float _height;

bool _flag = false;

 

float bac = 0,fr = 0,lef = 0 ,rig = 0,up = 0,down = 0;

 

const string COCKPIT_NAME = "MainCockpit";

const double MAX_DISTANCE = 2000;

const double GYRO_I = 30;

const double GYRO_D = 30;

const double MAX_ERROR = 0.1;

const double MAX_ANGLE_ERROR = 0.5;

const double MAX_VELOCITY_ERROR = 0.5;

const double GYRO_MULTEXTRA = 2;

const int GYRO_T = 30;

const float _ELE_MAX_ERROR = 2;

const float _MIN_VELOCITY = 2;

 

const float MAX_VELOCITY = 100;

 

const double THRUST_I = 30;

const double THRUST_D = 30;

const int THRUST_T = 30;

const double THRUST_MULTEXTRA = 1;

 

const double _extra = 100;

 

class PIDClass{

         

    public double getPID(double d){

         

        d_data.Add(d);

         

        if(d_data.Count == 1){

            d_data.Add(d);

        }

         

        if(d_data.Count > T){

            d_data.Remove(d_data[0]);

        }

        double sum_d = 0;

        foreach(double i in d_data){

            sum_d += i;

        }

         

        double fir = d + 1/I*(sum_d/d_data.Count) + D*(d_data[d_data.Count - 1] - d_data[d_data.Count - 2]);

         

        if(P2 == 0 || (fir >= 0 && P2 > 0))

            return P * fir; //输出结果

        else if(fir < 0 && P2 > 0){

            return P2*fir;

        }

        return 0;

    }

     

    public void setMultiplier(int t,double p,double i,double d){

        T = t;

        P = p;

        I = i;

        D = d;

    }

     

    public void setExtraMultiplier(double p2){

        P2 = p2;

    }

     

    public PIDClass(int t,double p,double i,double d){

        T = t;

        P = p;

        I = i;

        D = d;

    }

         

    private int T = 30; //周期，这个周期不是公式中的时间间隔T，而是储存累加结果的周期

    private double P = 0.0001; //比例系数

    private double I = 10; //积分系数

    private double D = 50; //微分系数

    private double P2 = 0;

     

    private List<double> d_data = new List<double>();

};

PIDClass gyroPIDX,gyroPIDY,gyroPIDZ;

PIDClass thrustPIDX,thrustPIDY,thrustPIDZ;

PIDClass thrustPIDY2,thrustPIDY3,thrustPIDZ2;

 

PIDClass VthrustPIDX,VthrustPIDY,VthrustPIDZ;

PIDClass VthrustPIDY2,VthrustPIDY3,VthrustPIDZ2;

PIDClass VthrustPIDX4,VthrustPIDY4,VthrustPIDZ4;

 

void PrintVector3D(Vector3D v){

    Echo("X="+Math.Round(v.X,4).ToString());

    Echo("Y="+Math.Round(v.Y,4).ToString());

    Echo("Z="+Math.Round(v.Z,4).ToString());

}

 

public void AddVector(Vector3D addvector,IMyShipController cockpit,List<IMyThrust> thrusts){

     

    float ShipMass = cockpit.CalculateShipMass().PhysicalMass;

     

    //MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(),cockpit.WorldMatrix.Forward, cockpit.WorldMatrix.Up);

    //addvector = Vector3D.TransformNormal(addvector,refLookAtMatrix);

     

    addvector = addvector*ShipMass;

     

    double totalG = addvector.Length() ;

     

     

    double totalthrust = 0;//debug

    foreach(var thrust in thrusts){

        thrust.ThrustOverride = 0;

        thrust.ThrustOverridePercentage = 0;

        var maxthrust = thrust.MaxEffectiveThrust;

        thrust.Enabled = false;

        if(!thrust.IsFunctional){

            continue;

        }

        switch (cockpit.WorldMatrix.GetClosestDirection(thrust.WorldMatrix.Backward))

        {

             

           //设定推力

            case Base6Directions.Direction.Backward:

                 

                thrust.ThrustOverridePercentage=(targetThrustPower.Z>0)?(float)targetThrustPower.Z:0;

                thrust.Enabled = (targetThrustPower.Z>0)?true:false;

                if(addvector.Z > 0 && !_dam ){

                    thrust.Enabled = true;

                     

                    float oldth = (float)thrust.ThrustOverride;

                    if(maxthrust <addvector.Z + thrust.ThrustOverride){

                        thrust.ThrustOverride = maxthrust;

                        addvector.Z -= maxthrust - oldth;

                    }

                    else{

                        thrust.ThrustOverride += (float)addvector.Z;

                        addvector.Z = 0;

                         

                    }

                     

                }

                 

            break;

            case Base6Directions.Direction.Forward:

                 

                thrust.ThrustOverridePercentage=(targetThrustPower.Z<0)?(float)-targetThrustPower.Z:0;

                thrust.Enabled = (targetThrustPower.Z<0)?true:false;

                if(addvector.Z < 0&& !_dam){

                    thrust.Enabled = true;

                    float fz = (-1) * (float)addvector.Z;

                    float oldth = (float)thrust.ThrustOverride;

                    if(maxthrust <fz + oldth){

                        thrust.ThrustOverride = maxthrust;

                        addvector.Z += maxthrust - oldth;

                    }

                    else{

                        thrust.ThrustOverride += fz;

                        addvector.Z = 0;

                         

                    }

                     

                }

             

            break;

            case Base6Directions.Direction.Right:

                 

                thrust.ThrustOverridePercentage=(targetThrustPower.X>0)?(float)targetThrustPower.X:0;

                thrust.Enabled = (targetThrustPower.X>0)?true:false;

                if(addvector.X > 0&& !_dam){

                    thrust.Enabled = true;

                    float oldth = (float)thrust.ThrustOverride;

                    if(maxthrust <addvector.X + oldth){

                        thrust.ThrustOverride = maxthrust;

                        addvector.X -= maxthrust - oldth;

                    }

                    else{

                        thrust.ThrustOverride += (float)addvector.X;

                        addvector.X = 0;

                         

                    }

                     

                }

                 

                 

            break;

            case Base6Directions.Direction.Left:

                 

                thrust.ThrustOverridePercentage=(targetThrustPower.X<0)?(float)-targetThrustPower.X:0;

                thrust.Enabled = (targetThrustPower.X<0)?true:false;

                if(addvector.X < 0&& !_dam){

                    thrust.Enabled = true;

                    float fz = (-1) * (float)addvector.X;

                    float oldth = (float)thrust.ThrustOverride;

                    if(maxthrust <fz + oldth){

                        thrust.ThrustOverride = maxthrust;

                        addvector.X += maxthrust - oldth;

                    }

                    else{

                        thrust.ThrustOverride += fz;

                        addvector.X = 0;

                         

                    }

                     

                }

                 

            break;

            case Base6Directions.Direction.Up:

                 

                thrust.ThrustOverridePercentage=(targetThrustPower.Y>0)?(float)targetThrustPower.Y:0;

                thrust.Enabled = (targetThrustPower.Y>0)?true:false;

                if(addvector.Y > 0&& !_dam){

                    thrust.Enabled = true;

                    float oldth = (float)thrust.ThrustOverride;

                    if(maxthrust <addvector.Y + oldth){

                        thrust.ThrustOverride = maxthrust;

                            addvector.Y -= maxthrust - oldth;

                    }

                    else{

                         

                        thrust.ThrustOverride += (float)addvector.Y;

                        addvector.Y = 0;

                         

                        int th = (int)thrust.ThrustOverride;

                         

                    }

                     

                }

             

            break;

            case Base6Directions.Direction.Down:

                 

                thrust.ThrustOverridePercentage=(targetThrustPower.Y<0)?(float)-targetThrustPower.Y:0;

                thrust.Enabled = (targetThrustPower.Y<0)?true:false;

                if(addvector.Y < 0&& !_dam){

                     

                    thrust.Enabled = true;

                    float fz = (-1) * (float)addvector.Y;

                    float oldth = (float)thrust.ThrustOverride;

                    if(maxthrust <fz + oldth){

                        thrust.ThrustOverride = maxthrust;

                        addvector.Y += maxthrust - oldth;

                    }

                    else{

                        thrust.ThrustOverride += fz;

                        addvector.Y = 0;

                         

                    }                      

                }

            break;

        }

        totalthrust += thrust.ThrustOverride;//debug

 

             

    }

}

 

void SetGyros(double pitch,double yaw,double roll){

     

    foreach(var gyro in _gyros){

        gyro.Enabled = true;

        gyro.GyroOverride = true;

        //gyro.Roll = (float)roll;

        //gyro.Pitch = (float)pitch;

        //gyro.Yaw = (float)yaw;

        if(pitch == 0 && yaw == 0 && roll == 0)

            gyro.GyroOverride = false;

        switch (_cockpit.WorldMatrix.GetClosestDirection(gyro.WorldMatrix.Backward))

        {//正右负左，正上负下

            case Base6Directions.Direction.Forward:

                gyro.Pitch = (float)-pitch;

                gyro.Yaw = (float)yaw;

                gyro.Roll = (float)-roll;

                break;

            case Base6Directions.Direction.Backward:

                gyro.Pitch = (float)pitch;

                gyro.Yaw = (float)yaw;

                gyro.Roll = (float)roll;

                break;

            case Base6Directions.Direction.Right:

                gyro.Pitch = (float)roll;

                gyro.Roll = (float)-pitch;

                gyro.Yaw = (float)yaw;

                break;

            case Base6Directions.Direction.Left:

                gyro.Pitch = (float)roll;

                gyro.Roll = (float)-pitch;

                gyro.Yaw = (float)yaw;

                break;

            case Base6Directions.Direction.Down:

                gyro.Pitch = (float)pitch;

                gyro.Roll = (float)-yaw;

                gyro.Yaw = (float)roll;

                break;

            case Base6Directions.Direction.Up:

                gyro.Roll = (float)yaw;

                gyro.Pitch = (float)-pitch;

                gyro.Yaw = (float)roll;

                break;

        }

    }

     

     

}

 

bool InitGyros(){//测陀螺仪最大角加速度

    if(_MaxAngleAcceleration.Length() != 0){

        //若已经测过了，返回

        return true;

    }

     

    Vector3D angularVelocity = _cockpit.GetShipVelocities().AngularVelocity;

    MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Forward, _cockpit.WorldMatrix.Up);

    Vector3D meAngleVelocityToMe = Vector3D.TransformNormal(angularVelocity, refLookAtMatrix);

    //(*)-

    double meRollAngleVelocity = meAngleVelocityToMe.Z*180/Math.PI; //当前角速度

    double mePitchAngleVelocity = meAngleVelocityToMe.X*180/Math.PI;

    double meYawAngleVelocity = meAngleVelocityToMe.Y*180/Math.PI;

    //得到当前帧的角速度(*)-

    if(_InitAngleVelocity.Length() == 0){

        _InitAngleVelocity = new Vector3D(mePitchAngleVelocity,meYawAngleVelocity,meRollAngleVelocity);

 

        SetGyros(60,60,60);

    }

    else{

        _MaxAngleAcceleration = new Vector3D((mePitchAngleVelocity - _InitAngleVelocity.X)*60,(meYawAngleVelocity - _InitAngleVelocity.Y)*60,(meRollAngleVelocity - _InitAngleVelocity.Z)*60);

        //计算最大角加速度，由于一秒60帧所以乘60

        //gys.SetOverride(false);

        //gys.Yaw = gys.Pitch = 0;

        //返回原状态(*)-

        SetGyros(0,0,0);

        double P = 60/(GYRO_T*(1/GYRO_I*180+GYRO_D*180))*GYRO_MULTEXTRA;

        gyroPIDX.setMultiplier(GYRO_T,P,1/GYRO_I,GYRO_D);

        gyroPIDY.setMultiplier(GYRO_T,P,1/GYRO_I,GYRO_D);

        gyroPIDZ.setMultiplier(GYRO_T,P,1/GYRO_I,GYRO_D);

    }

     

    return false;

}

 

bool gyroOperation(Vector3D target){

    Vector3D err = new Vector3D(target.X,target.Y,target.Z);

    if(err.Length() <= MAX_ERROR)

        return true;

    Vector3D nowtarget = new Vector3D(gyroPIDX.getPID(target.X),-gyroPIDY.getPID(target.Y),gyroPIDZ.getPID(target.Z));

    SetGyros(nowtarget.X,nowtarget.Y,nowtarget.Z);

    return false;

}

 

public static double TargetAngleToMe(Vector3D targetToMe,string direction){

    targetToMe = Vector3D.Normalize(targetToMe);

    Vector3D zForward = new Vector3D(0,0,-1);

     

    targetToMe.X *= -1;//SE中，前负后正，右负左正，上负下正

    targetToMe.Y *= -1;

    targetToMe.Z *= -1;

     

    if(direction == "Yaw"){

         

        Vector3D targetYawVector = new Vector3D(targetToMe.X,0,targetToMe.Z);

         

        return Math.Atan2(targetToMe.X,targetToMe.Z) * 180/Math.PI;

         

    }

    else if (direction == "Pitch"){

        Vector3D targetYawVector = new Vector3D(0,targetToMe.Y,targetToMe.Z);

        return Math.Atan2(targetToMe.Y,targetToMe.Z) * 180/Math.PI;

         

         

    }

    else if(direction == "Roll"){

         

        Vector3D targetRollVector = new Vector3D(targetToMe.X,targetToMe.Y,0);

         

        return Math.Atan2(targetToMe.X,targetToMe.Y) * 180/Math.PI;

         

    }

     

    return 0;

}

 

Vector3D GetTarget(string str){

    //解析坐标

    return new Vector3D(0,0,0);

}

 

public Program()

{

    // 构造函数，每次脚本运行时会被首先调用一次。用它来初始化脚本。

    //  构造函数是可选项，

    // 如不需要可以删除。

    //

    // 建议这里设定RuntimeInfo.UpdateFrequency，

    // 这样脚本就不需要定时器方块也能自动运行了。

    Runtime.UpdateFrequency = UpdateFrequency.Update1;

     

    _MaxAngleAcceleration = new Vector3D();

    _InitAngleVelocity = new Vector3D();

    debug = 0;

     

    gyroPIDX = new PIDClass(GYRO_T,1,1/GYRO_I,GYRO_D);

    gyroPIDY = new PIDClass(GYRO_T,1,1/GYRO_I,GYRO_D);

    gyroPIDZ = new PIDClass(GYRO_T,1,1/GYRO_I,GYRO_D);

 

     

    GridTerminalSystem.GetBlocksOfType<IMyGyro>(_gyros);

    GridTerminalSystem.GetBlocksOfType<IMyShipController>(_shipControls);

    GridTerminalSystem.GetBlocksOfType<IMyCameraBlock>(_camera);

    GridTerminalSystem.GetBlocksOfType<IMyThrust>(_thrusts);

     

    foreach(var cam in _camera){

        cam.EnableRaycast = true;

    }

     

    bool flag = false;

    foreach(var coc in _shipControls){

        if(coc.CustomName == COCKPIT_NAME){

            _cockpit = coc;

            flag = true;

        }

    }

    if(!flag) _cockpit = _shipControls[0];

     

    MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Forward, _cockpit.WorldMatrix.Up);

     

    SetTarget();

    /*if(_cockpit.GetNaturalGravity().Length() != 0){

        Vector3D v3d = new Vector3D();

         

        _cockpit.TryGetPlanetPosition(out v3d);

         

        Vector3D tar_p = Vector3D.Normalize(t - v3d);

        Vector3D me_p = Vector3D.Normalize(_cockpit.CenterOfMass - v3d);

             

        double angle = Math.Abs(Math.Acos(Vector3D.Dot(tar_p,me_p)/Math.Abs(tar_p.Length()*me_p.Length()))*180/Math.PI);

         

        v3d = Vector3D.TransformNormal(t - v3d,refLookAtMatrix);

        _height = (float)v3d.Length();

         

        double ele = 0;

        _cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface,out ele);

    }*/

     

     

     

    foreach(var thrust in _thrusts){

        switch (_cockpit.WorldMatrix.GetClosestDirection(thrust.WorldMatrix.Backward))

        {

            case Base6Directions.Direction.Backward:

                bac += thrust.MaxEffectiveThrust;

                break;

            case Base6Directions.Direction.Forward:

                fr += thrust.MaxEffectiveThrust;

                break;

            case Base6Directions.Direction.Left:

                lef += thrust.MaxEffectiveThrust;

                break;

            case Base6Directions.Direction.Right:

                rig += thrust.MaxEffectiveThrust;

                break;

            case Base6Directions.Direction.Up:

                up += thrust.MaxEffectiveThrust;

                _maxThrust += thrust.MaxEffectiveThrust;

                break;

            case Base6Directions.Direction.Down:

                down += thrust.MaxEffectiveThrust;

                break;

             

        }

    }

    bac /= _cockpit.CalculateShipMass().PhysicalMass;

    fr /= _cockpit.CalculateShipMass().PhysicalMass;

    lef /= _cockpit.CalculateShipMass().PhysicalMass;

    rig /= _cockpit.CalculateShipMass().PhysicalMass;

    up /= _cockpit.CalculateShipMass().PhysicalMass;

    down /= _cockpit.CalculateShipMass().PhysicalMass;

     

    Vector3D distance = t - _cockpit.CenterOfMass;

    t = Vector3D.TransformNormal(distance, refLookAtMatrix);

     

    if(distance.X < 0) distance.X = -distance.X;

    if(distance.Y < 0) distance.Y = -distance.Y;

    if(distance.Z < 0) distance.Z = -distance.Z;

     

    thrustPIDX = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*lef/((THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);

    thrustPIDY = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*down/((THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);

    thrustPIDZ = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*bac/((THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);

     

     

    //debug2 = angle;

     

    VthrustPIDX = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.X),THRUST_I,THRUST_D);

    VthrustPIDY = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.Y),THRUST_I,THRUST_D);

    VthrustPIDZ = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.Z),THRUST_I,THRUST_D);

     

    VthrustPIDX4 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.X),THRUST_I,THRUST_D);

    VthrustPIDY4 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.Y),THRUST_I,THRUST_D);

    VthrustPIDZ4 = new PIDClass(THRUST_T,10*THRUST_MULTEXTRA*MAX_VELOCITY/((THRUST_I+THRUST_D+1)*distance.Z),THRUST_I,THRUST_D);

     

     

    thrustPIDX.setExtraMultiplier(20*THRUST_MULTEXTRA*rig/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));

    thrustPIDY.setExtraMultiplier(20*THRUST_MULTEXTRA*up/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));

    thrustPIDZ.setExtraMultiplier(20*THRUST_MULTEXTRA*fr/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));

     

    _cockpit.DampenersOverride = false;

     

     

     

}

 

public void Save()

{

    // 当程序需要保存时，会发出提醒。使用这种方法可以将状态保存

    // 至内存或其他路径。此为备选项，

    // 如果不需要，可以删除。

    foreach(var thrust in _thrusts){

        thrust.Enabled = true;

        thrust.ThrustOverridePercentage = 0;

         

    }

    foreach(var gyro in _gyros){

        gyro.Enabled = true;

        gyro.GyroOverride = false;

    }

}

 

void SetTarget()

{

    t = TARGET;

}

 

float GetH(float v){

     

    MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Forward, _cockpit.WorldMatrix.Up);

     

     

     

    float ShipMass = _cockpit.CalculateShipMass().PhysicalMass;

     

    Vector3D g = _cockpit.GetNaturalGravity();

    float totalg = (float)g.Length();

     

    float upv = (float)((_maxThrust - ShipMass*totalg) / (ShipMass));

    //Echo("upv" + upv.ToString());

     

     

    Vector3D V = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix);

    float vv = (float)V.Length();

     

    float nowv = vv;

    float t = vv/upv;

    float x = (float)(vv*t - 0.5*upv*t*t);

    float totalh = x;

    Echo("x = " + x.ToString());

     

     

    return totalh + (float)_extra;

}

 

public void Main(string argument, UpdateType updateSource)

{

     

    /*foreach(var thrust in _thrusts){

        thrust.Enabled = true;

        thrust.ThrustOverridePercentage = 0;

         

    }

    foreach(var gyro in _gyros){

        gyro.Enabled = true;

        gyro.GyroOverride = false;

    }

     

    return;*/

    Echo("State="+_state.ToString());

     

    targetThrustPower = new Vector3D();

     

    if(InitGyros() == false) return;

    _dam = _cockpit.DampenersOverride;

     

    MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Forward, _cockpit.WorldMatrix.Up);

     

    Vector3D V = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix);

     

    SetTarget();

     

     

    Vector3D g = _cockpit.GetNaturalGravity();

    float totalg = (float)g.Length();

     

 

    if(_dam == true) targetThrustPower=-Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix);

     

    if(totalg == 0)

    {

        t = t - _cockpit.CenterOfMass;

     

         

        t = Vector3D.TransformNormal(t, refLookAtMatrix);

        //

 

        Vector3D tar = new Vector3D(TargetAngleToMe(t,"Pitch"),TargetAngleToMe(t,"Yaw"),0);

        AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);

         

        if(!gyroOperation(tar)) return;

         

        if(!_cockpit.DampenersOverride)targetThrustPower = new Vector3D(0,0,0);

 

        foreach(var cam in _camera){

            int timebeforeray = cam.TimeUntilScan(MAX_DISTANCE);

            if(timebeforeray > 0)

                continue;

            MyDetectedEntityInfo info = cam.Raycast(MAX_DISTANCE,0,0);

            if(!info.IsEmpty())

                _cockpit.DampenersOverride = true;

        }

         

 

        if(!_cockpit.DampenersOverride){

            //Vector3D a = new Vector3D(-thrustPIDX.getPID(t.X),thrustPIDY.getPID(t.Y),thrustPIDZ.getPID(t.Z));

            double tarv = VthrustPIDZ4.getPID(-t.Z);

            double tara = thrustPIDZ.getPID(tarv + V.Z);

            Vector3D a = new Vector3D(0,0,-tara);

            Echo("tarv = " + (tarv+V.Z).ToString());

            Echo("tara = " + tara.ToString());

            if(Math.Abs(Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix).Z) >= 99)

                a = new Vector3D(0,0,0);

            AddVector(a,_cockpit,_thrusts);

        }

    }

    else{

        if(_flag == false) {

            _state = -1;

             

             

            Vector3D v3d = new Vector3D();

         

            _cockpit.TryGetPlanetPosition(out v3d);

             

            Vector3D tar_p = Vector3D.Normalize(t - v3d);

            Vector3D me_p = Vector3D.Normalize(_cockpit.CenterOfMass - v3d);

                 

            double angle = Math.Abs(Math.Acos(Vector3D.Dot(tar_p,me_p)/Math.Abs(tar_p.Length()*me_p.Length()))*180/Math.PI);

             

            v3d = Vector3D.TransformNormal(_cockpit.CenterOfMass - v3d,refLookAtMatrix);

            _height = (float)v3d.Length();

             

            double ele = 0;

            _cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface,out ele);

             

             

            thrustPIDY2 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*down/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);

            thrustPIDY3 = new PIDClass(THRUST_T,10*THRUST_MULTEXTRA*down/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);

            //debug2 = THRUST_MULTEXTRA*down/((1/THRUST_I+THRUST_D+1)*_height);

            thrustPIDZ2 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*bac/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY),THRUST_I,THRUST_D);

             

            VthrustPIDY2 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((1/THRUST_I+THRUST_D+1)*_height),THRUST_I,THRUST_D);

            VthrustPIDY3 = new PIDClass(THRUST_T,30*THRUST_MULTEXTRA*MAX_VELOCITY/((1/THRUST_I+THRUST_D+1)*ele),THRUST_I,THRUST_D);

            //debug2 = THRUST_MULTEXTRA*down/((1/THRUST_I+THRUST_D+1)*_height);

            VthrustPIDZ2 = new PIDClass(THRUST_T,20*THRUST_MULTEXTRA*MAX_VELOCITY/((1/THRUST_I+THRUST_D+1)*angle),THRUST_I,THRUST_D);

             

            thrustPIDY2.setExtraMultiplier(20*THRUST_MULTEXTRA*up/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));

            thrustPIDY3.setExtraMultiplier(10*THRUST_MULTEXTRA*up/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));

            //debug2 = THRUST_MULTEXTRA*up/ele;

            thrustPIDZ2.setExtraMultiplier(20*THRUST_MULTEXTRA*fr/((1/THRUST_I+THRUST_D+1)*MAX_VELOCITY));

        }

        _flag = true;

        if(_state == -1){

            double v = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix).Length();

            if(v > MAX_VELOCITY_ERROR) _cockpit.DampenersOverride = true;

            else {

                _state = 0;

                _cockpit.DampenersOverride = false;

            }

            AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);

        }

        else if(_state == 0){

            Vector3D plantPosition;

            _cockpit.TryGetPlanetPosition(out plantPosition);

             

            Vector3D tar_p = t - plantPosition;

            Vector3D me_p = _cockpit.CenterOfMass - plantPosition;

             

            double angle = Math.Abs(Math.Acos(Vector3D.Dot(tar_p,me_p)/Math.Abs(tar_p.Length()*me_p.Length()))*180/Math.PI);

            t = Vector3D.TransformNormal(t,refLookAtMatrix);

             

            g = Vector3D.TransformNormal(g,refLookAtMatrix);

            Vector3D gv = new Vector3D(TargetAngleToMe(g,"Pitch")-90,TargetAngleToMe(t,"Yaw"),TargetAngleToMe(g,"Roll"));

            AddVector(-g,_cockpit,_thrusts);

            if(!gyroOperation(gv)) return;

             

            Vector3D moveg = -g;

            double verr = VthrustPIDZ2.getPID(angle)+V.Z;

            moveg.Z = -thrustPIDZ2.getPID(verr);//解决，双重PID

            Echo("angle_err = "+angle.ToString());

            Echo("velocity_err = " + verr.ToString());

            Echo("PID outcome = " + moveg.Z.ToString());

             

            double z_v = V.Z;

            if(Math.Abs(z_v) >= MAX_VELOCITY-1 && z_v * moveg.Z > 0) moveg.Z = 0;

             

            plantPosition = Vector3D.TransformNormal(_cockpit.CenterOfMass-plantPosition,refLookAtMatrix);

            double vverr = VthrustPIDY2.getPID(_height - plantPosition.Length())-V.Y;

            double y2 = thrustPIDY2.getPID(vverr);//高了为负，待测试

            Echo("height_err = " + (_height - plantPosition.Length()).ToString());

            Echo("verr = " + vverr.ToString());

            Echo("PID outcome = " + y2.ToString());

            moveg.Y += y2;

            moveg.Y += z_v*z_v/plantPosition.Length();//***向心力

             

            AddVector(moveg,_cockpit,_thrusts);

            if(angle <= MAX_ANGLE_ERROR) _state = 1;

             

        }

        else if(_state == 1){

            double v = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix).Length();

            if(v > MAX_VELOCITY_ERROR) _cockpit.DampenersOverride = true;

            else {

                _state = 2;

                _cockpit.DampenersOverride = false;

            }

            AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);

        }

        else if(_state == 2){

            /* PID方法 *

            double ele = 0;

            _cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface,out ele);

             

            g = Vector3D.TransformNormal(g,refLookAtMatrix);

            Vector3D gv = new Vector3D(TargetAngleToMe(g,"Pitch")-90,0,TargetAngleToMe(g,"Roll"));

            gyroOperation(gv);

             

            double verr = VthrustPIDY3.getPID(ele)-V.Y;

            Vector3D moveg = new Vector3D(0,-thrustPIDY3.getPID(verr)-g.Y,0);//待测试***

            Echo("ele err = " + ele.ToString());

            Echo("tar v = " + (verr+V.Y).ToString());

            Echo("PID outcome = " + (moveg.Y+g.Y).ToString());

             

             

             

            double y_v = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix).Y;

             

            if(y_v > MAX_VELOCITY-1 && y_v*moveg.Y > 0) moveg.Y = 0;

             

             

            AddVector(moveg,_cockpit,_thrusts);*/

             

            double ele = 0;

            _cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface,out ele);

             

            float minh = GetH((float)_cockpit.GetShipSpeed());

             

            g = Vector3D.TransformNormal(g,refLookAtMatrix);

            g = Vector3D.Normalize(g);

            Vector3D gv = new Vector3D(TargetAngleToMe(g,"Pitch")-90,0,TargetAngleToMe(g,"Roll"));

            gyroOperation(gv);

             

            AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);

             

            //Vector3D V = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity,refLookAtMatrix);

             

            if(ele <= _ELE_MAX_ERROR ){

                _state = 3;

            }

            else if(ele <= minh && V.Y < 0 && -V.Y >= _MIN_VELOCITY){

                targetThrustPower = -g;

                AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);

            }

             

        }

        else if(_state == 3){

            AddVector(new Vector3D(0,0,0),_cockpit,_thrusts);

            g = Vector3D.TransformNormal(g,refLookAtMatrix);

            g = Vector3D.Normalize(g);

            Vector3D gv = new Vector3D(TargetAngleToMe(g,"Pitch")-90,TargetAngleToMe(t,"Yaw"),TargetAngleToMe(g,"Roll"));

            gyroOperation(gv);

        }

    }

         

}