Space Engineers - Flight scripts

AutoLevel.cs

// Set name for remote control to orient on.
// Null to use first found remote control.
string REMOTE_CONTROL_NAME = null;

// Set lower if overshooting, set higher to respond quicker
double CTRL_COEFF = 0.8d;

// Set to the max number of gyros to use
// Using less gyros than you have allows you to still steer while leveler is operating.)
int LIMIT_GYROS = 999;

////////////////////////////////////////////////////////////

IMyRemoteControl _rc;
List<IMyGyro> _gyros;

void Main(string args)
{
	if(_rc==null)
		Setup();
		
	//Get orientation from _rc
	Matrix or;
	_rc.Orientation.GetMatrix(out or);
	Vector3D down = or.Down;

	Vector3D grav = _rc.GetNaturalGravity();
	grav.Normalize();

	for(int i=0; i < _gyros.Count; ++i)
	{
		var g = _gyros[i];
		
		g.Orientation.GetMatrix(out or);
		var localDown = Vector3D.Transform(down, MatrixD.Transpose(or));
		var localGrav = Vector3D.Transform(grav, MatrixD.Transpose(g.WorldMatrix.GetOrientation()));
		
		//Since the gyro ui lies, we are not trying to control yaw,pitch,roll but rather we
		//need a rotation vector (axis around which to rotate)
		var rot = Vector3D.Cross(localDown, localGrav);
		double ang = rot.Length();

        // More numerically stable than: ang=Math.Asin(ang)
        // Also much faster \o/
		ang = Math.Atan2(ang, Math.Sqrt(Math.Max(0.0, 1.0-ang*ang)));

		if( ang < 0.01)
		{   //Close enough
			//Echo("Level");
			g.SetValueBool("Override", false);
			continue;
		}
		//Echo("Off level: "+(ang*180.0/3.14).ToString()+"deg");
		
		//Control speed to be proportional to distance (angle) we have left
		double ctrl_vel = g.GetMaximum<float>("Yaw")*(ang/Math.PI)*CTRL_COEFF;

		ctrl_vel = Math.Min(g.GetMaximum<float>("Yaw"), ctrl_vel);
		ctrl_vel = Math.Max(0.01, ctrl_vel); //Gyros don't work well at very low speeds
		rot.Normalize();

		rot *= ctrl_vel;
		g.SetValueFloat("Pitch", (float)rot.GetDim(0));
		g.SetValueFloat("Yaw", -(float)rot.GetDim(1));
		g.SetValueFloat("Roll", -(float)rot.GetDim(2));
		
        // Full power cap'n
		g.SetValueFloat("Power", 1.0f);
		g.SetValueBool("Override", true);
	}
}

void Setup()
{
	var l = new List<IMyTerminalBlock>();
	
	_rc = (IMyRemoteControl)GridTerminalSystem.GetBlockWithName(REMOTE_CONTROL_NAME ?? "");
	if(_rc == null)
	{
		GridTerminalSystem.GetBlocksOfType<IMyRemoteControl>(l, x => x.CubeGrid == Me.CubeGrid);
		_rc = (IMyRemoteControl)l[0];
	}
	
	GridTerminalSystem.GetBlocksOfType<IMyGyro>(l, x => x.CubeGrid == Me.CubeGrid);
	gyros = l.ConvertAll(x => (IMyGyro)x);

	if(gyros.Count > LIMIT_GYROS)
		gyros.RemoveRange(LIMIT_GYROS,gyros.Count-LIMIT_GYROS);
}