【不务正业】太空工程师自动导航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);
		}
	}
		
}