Find Pitch and Yaw to target

Hello.
i want to understand to know pitch and yaw for facing to target by gyro.
like how guidance missile work but i don't understand how it work.
please explain me how these math algorithm work ...

Thank you very much for helping.

//sorry my ENG is bad

 

You will have to do some trigonometry. Here's my code for it.



// https://gamedev.stackexchange.com/qu...global-coordinate-space-to-a-local-space?rq=1
// using a turret as a sensor for chasing enemies with. Guaranteed to at least get you in range of a vanilla 50m sensor.

// turret should be mounted so that it's facing down when it gets built, and its field of fire covers the forward half of the ship.
// remote should be mounted roughly behind the turret and facing forward. Alignment is not mandatory.
string displayName = "MT Display"; // name of debug display; optional
string turretName = "MT Turret"; // name of turret
string remoteName = "MT Remote"; // this is what does the standard patrol until the turret picks something up
const double AziMult = 1; // adjust these if you absolutely can't install the turret facing forward by default and pointing up
const double EleMult = 1; // adjust these if you absolutely can't install the turret facing forward by default and pointing up
const double MaxNegativeElevation = -10; // IN DEGREES! since the turret is mounted facing up, it cannot pitch downwards more than a little bit. So, use that to figure out if we should do an about face.
const double HeadFwdThreshold = 70; // IN DEGREES! Same as a bove: if the elevation is this high or more, we're aimed at our target well enough to justify continuing on present course (prevents overcorrection)
const double FwdFireThreshold = 85; // IN DEGREES! Same as a bove: if the elevation is this high or more, we're aimed at our target well enough to justify turning on forward-facing weapons, or doing evasion, etc.
int ticks=0; // system ticks
bool AmITrackingSomething=false;
bool AmITailingSomething=false;
double curA,curE,prevA,prevE=0;

const double tau=2*Math.PI;
const double pi6=Math.PI/6;
const double pi3=Math.PI/3;
const double deg=57.295779513082320876798154814105; // radians to degrees
const double rad=0.01745329251994329576923690768489; // degrees to radians



bool DetermineTracking(IMyLargeTurretBase t) // only run me once per loop!
{
if (t==null)
return false;
prevA=curA;
prevE=curE;
curA=t.Azimuth;
curE=t.Elevation;
AmITrackingSomething = !((prevA==curA)&&(prevE==curE));
return AmITrackingSomething;
}

// azimuth seems to be between -pi and +pi



void Main(String order) {
Print("Debug Info");
IMyLargeTurretBase turr = (IMyLargeTurretBase)GridTerminalSystem.GetBlockWithName(turretName);
if (turr == null){
Print("ERROR! Name your guidance turret "+turretName);
return;
}

AmITrackingSomething=DetermineTracking(turr);
IMyRemoteControl rr = GridTerminalSystem.GetBlockWithName(remoteName) as IMyRemoteControl;

double TurWeapAzi=curA*AziMult;
double TurWeapEle=curE*EleMult;

double TurBaseAzi=0;
double TurBaseEle=0;
// var TurBaseUp = turr.Position - turr.Position + Base6Directions.GetIntVector(turr.Orientation.TransformDirection(Base6Directions.Direction.Up));
// var TurBaseUpVector = turr.CubeGrid.GridIntegerToWorld(TurBaseUp);
// var TurBaseUpVectorNormalize = Vector3D.Normalize(TurBaseUpVector - turr.GetPosition()); // okay, this gives me the vector along which the turret base is pointed at (turret up is ship front)

Vector3D TurBaseUpVectorNormalize = Vector3D.Normalize(turr.CubeGrid.GridIntegerToWorld(Base6Directions.GetIntVector(turr.Orientation.TransformDirection(Base6Directions.Direction.Up))) - turr.GetPosition()); // okay, this gives me the vector along which the turret base is pointed at (turret up is ship front)
Vector3D TurBaseFrontVectorNormalize = Vector3D.Normalize(turr.CubeGrid.GridIntegerToWorld(Base6Directions.GetIntVector(turr.Orientation.TransformDirection(Base6Directions.Direction.Forward))) - turr.GetPosition()); // okay, this gives me the vector along which the turret base is pointed at (turret up is ship front)
Vector3D TurBaseRightVectorNormalize = Vector3D.Normalize(turr.CubeGrid.GridIntegerToWorld(Base6Directions.GetIntVector(turr.Orientation.TransformDirection(Base6Directions.Direction.Right))) - turr.GetPosition()); // okay, this gives me the vector along which the turret base is pointed at (turret up is ship front)

Vector3D RemFwdVectorNormalize = Vector3D.Normalize(rr.CubeGrid.GridIntegerToWorld(Base6Directions.GetIntVector(rr.Orientation.TransformDirection(Base6Directions.Direction.Forward))) - rr.GetPosition());

Vector3D.GetAzimuthAndElevation(TurBaseUpVectorNormalize, out TurBaseAzi, out TurBaseEle); // azi/ele of turret base
Vector3D TurWeapVectorNormalize = new Vector3D(0,0,0);
Vector3D.CreateFromAzimuthAndElevation(TurWeapAzi,TurWeapEle, out TurWeapVectorNormalize); // normalized vector from weapon azi/ele


Print("Remo V:"+RemFwdVectorNormalize.X+" "+RemFwdVectorNormalize.Y+" "+RemFwdVectorNormalize.Z);
Print("Base V:"+TurBaseUpVectorNormalize.X+" "+TurBaseUpVectorNormalize.Y+" "+TurBaseUpVectorNormalize.Z);
Print("Weap V:"+TurWeapVectorNormalize.X+" "+TurWeapVectorNormalize.Y+" "+TurWeapVectorNormalize.Z);

Vector3D.GetAzimuthAndElevation(TurBaseUpVectorNormalize,out TurBaseAzi, out TurBaseEle);

Double rng = turr.Range;
Vector3D arrow = new Vector3D(0,0,0);

//v1 = turret mapped onto ship
//v2 = ship
//v3 = origin
//v4x = (v1x)*(v2x)+(v1y)*(v2x)+(v1z)*(V2x)

// https://www.kwon3d.com/theory/transform/transform.html

//v4 = turret mapped onto origin

// I can't figure out the matrix stuff, so we're going to do linear interpolation from known values.


// v2 means sqrt(2)/2
// if turret is pointing front, aimvec is 0 1 0. This should leave the arrow alone. a/e is (any number), pi/2 in theory, 1.55 in practice
// if turret is pointing down, aimvec is 0 0 -1, a/e is 0 0
// if turret is pointing up, aimvec is 0 0 1, a/e is -pi, 0
// if turret is pointing left, aimvec is -1 0 0, a/e is pi/2, 0
// if turret is pointing right, aimvec is 1 0 0 a/e is -pi/2, 0
// if turret is pointing front-down, aimvec is 0 v2 -v2, a/e is 0, pi/4
// if turret is pointing front-up, aimvec is 0 v2 v2, a/e is -pi, pi/4
// if turret is pointing front-left, aimvec is -v2 v2 0 a/e is pi/2, pi/4
// if turret is pointing front-right, aimvec is v2 v2 0, a/e is -pi/2, pi/4


// normalize azimuth
while (TurWeapAzi<tau)
TurWeapAzi=TurWeapAzi+tau;
TurWeapAzi=TurWeapAzi%(tau);


// What we're going to do is see what the turret is closest to, and direct the main ship accordingly.

// if (TurWeapEle>0.9)
// arrow=TurBaseFrontVectorNormalize*rng // go straight, good enough

// Convert to degrees just to make my life a bit easier. Azi is 0 to 359 and ele is -10 to 90, except the turret has a tiny dead cone so it's more like -10 to 89.


// D: 360/0
// DR: 315
// R: 270
// UR: 225
// U: 180
// UL: 135
// L: 90
// DL: 45


// special case: if elevation is as far back (down) as it can go, order the system to do two thirds of an about face in order to reaquire next tick
if (TurWeapEle<(MaxNegativeElevation*0.9*rad))
{
TurWeapEle=-60*rad;
}

if (TurWeapEle>(HeadFwdThreshold*rad))
{
arrow = TurBaseUpVectorNormalize; // just go straight
}
else
{
// first map the azimuth to cos and sin of FRONT and LEFT
arrow=TurBaseFrontVectorNormalize*Math.Cos(TurWeapAzi)-TurBaseRightVectorNormalize*Math.Sin(TurWeapAzi);
// then map the elevation
arrow=arrow*Math.Cos(TurWeapEle)+TurBaseUpVectorNormalize*Math.Sin(TurWeapEle);
}



AmITailingSomething=(TurWeapEle>(FwdFireThreshold*rad));

TurWeapAzi*=deg; // for vis
TurWeapEle*=deg; // for vis


arrow=(Vector3D.Normalize(arrow));
Print("Arro V:"+arrow.X+" "+arrow.Y+" "+arrow.Z);
arrow=arrow*(rng/2); // move turretrange // move10m

// Vector3D arrow = new Vector3D((TurBaseUpVectorNormalize.X+aimvec.X)*rng, (TurBaseUpVectorNormalize.Y+aimvec.Y)*rng, (TurBaseUpVectorNormalize.Z+aimvec.Z)*rng);
Vector3D headtome = rr.GetPosition()+arrow;
Vector3D rp = turr.GetPosition();
Print("TPos V:"+rp.X+" "+rp.Y+" "+rp.Z);
Print("Weap AE:"+TurWeapAzi+" "+TurWeapEle);
Print("Base AE:"+TurBaseAzi+" "+TurBaseEle);
Print("Trk: "+AmITrackingSomething+" Tail: "+AmITailingSomething+" C:"+ticks);













// And this is where we do any actual control, delegating the low level stuff to the remote block.

if (AmITrackingSomething)
{
rr.ClearWaypoints();
rr.AddWaypoint(headtome,"pos+arrow");
rr.SetAutoPilotEnabled(true);
Print ("Est Target Pos:"+headtome.ToString());
}
else
{
Print("Not tracking");
rr.SetAutoPilotEnabled(false);
}

FinalizePrint();
ticks++;
}


string DisplayString="";
void Print(string p) {
Echo(p);
DisplayString=DisplayString+p+"\n";
}

void FinalizePrint()
{
IMyTextPanel t = (IMyTextPanel)GridTerminalSystem.GetBlockWithName(displayName);
if (t!=null)
t.WritePublicText(DisplayString);
DisplayString="";
}

 

Thank you very much!!

 

Great reply. But I'm not sure if this answers the original question as it uses the remote to establish the direction of the grid. What about using gyros to point the ship to a particular heading, such as a GPS location and keep the grid zero'ed in on that heading using the gyros? I think @Whiplash141 's optically guided missiles use this set up.

Using the remote control feature can yield in mixed results. It's a bit buggy if there aren't enough thrusters in every direction. It may also attempt to stop or slow down if the destination GPS is exactly on the target. This is bad for missiles that are attempting to collide with a target and minimize the amount of time it is exposed to incoming fire.

 

Whiplash has been summoned

Howdy!

So lets go over the basics of making something rotate in the direction you want.

• You need a set of reference axes. I like to use a remote control block to get these since it is easy to extract the forward, up, and right directions from the WorldMatrix.
  
  
• Take your desired target direction vector and project it onto the front-right plane and the front-up plane.
  
  
• Find the angle between the projected vectors and the front vector
  • cosine formula: a*b = |a| |b| cos(angle) --> angle = arccos( a*b / (|a| |b|))
    
• Determine if the pitch and yaw angles are positive or negative
  • dotProductPitch = targetVector * upVector if (dotProductPitch < 0) //this means that the vector is pointing downwards pitchAngle = pitchAngle* -1
    
    
  • dotProductYaw = targetVector * upVector
    
    if (dotProductYaw < 0) //this means that the vector is pointing downwards yawAngle = yawAngle* -1
    
• Apply gyroscope overrides accordingly

 

Beetlejuice, Beetlejuice, Beetlejuice!

As far as overriding your gyros, what is the most effective means for getting them to stop at the correct angle assuming you're wanting to point at one specific GPS coordinate?

 

Hehe

I use a PID angle controller (with no I because the integral term gets stupid hard to control).

Basis of PID control approximated in a discrete time system:

• P (Proportional) term: constant1 * angleToTarget
• I (Intergal) term: constant2 * errorSum (I normally ignore this term by making constant2 = 0. It takes a but-load of tuning otherwise)
  • errorSum = errorSum + angleToTarget * timeStep (This is a sum of all weighted past errors, this starts at 0 and gets larger)
• D (Derivative or Damping) term: constant3 * (angleToTarget - lastAngleToTarget)/ timeStep (This is important as it dampens the rotation reducing overshoot)

 

I think I'm confusing myself trying to do this - I know how to project a vector onto another vector, but how do I project the vector onto a plane? Fairly sure I'm over-complicating it.

EDIT: I just dissected your missile script I was definitely over-complicating it lol, I think I'm good now!

 

Yeah. For those who were also wondering: To project a vector onto a plane, you project that vector onto the normal of the plane, then subtract that projection from the original vector.

Pseudocode:

Code:

normalVec = project vector on planeNormal
flattenedVec = vector - normalVec