void S_S_M::gimbalengines(double simdt)
{
if(GetThrusterGroupLevel(THGROUP_MAIN) !=0)
{
pitch = GetThrusterGroupLevel(THGROUP_ATT_PITCHUP) - GetThrusterGroupLevel(THGROUP_ATT_PITCHDOWN);
lpitchangle = pitch*(PI/12);
rpitchangle = lpitchangle;
yaw = GetThrusterGroupLevel(THGROUP_ATT_YAWLEFT) - GetThrusterGroupLevel(THGROUP_ATT_YAWRIGHT); //right is positive
lyawangle = yaw*(PI/12);
ryawangle = lyawangle;
rollA = GetThrusterGroupLevel(THGROUP_ATT_BANKRIGHT)- GetThrusterGroupLevel(THGROUP_ATT_BANKLEFT);
if(rollA != 0)
{
if(pitch > 0)
{
if(rollA > 0) //roll right
{
lpitchangle -= (rollA*lpitchangle);
//drop left engine;
}
if(rollA < 0) //roll left
{
rpitchangle += (rollA*rpitchangle);
//drop right engine;
}
}
else if(pitch < 0)
{
if(rollA > 0) //roll right
{
rpitchangle -= (rollA*rpitchangle);
//raise right engine;
}
if(rollA < 0) //roll left
{
lpitchangle += (rollA*lpitchangle);
//raise left engine;
}
}
else
{
lpitchangle = -rollA*(PI/12);
rpitchangle = rollA*(PI/12);
}
}
if(pitch!=0 || yaw!=0 || rollA !=0)
{
gimf = simdt * GIMBAL_SPEED;
if(lcurrentpangle - lpitchangle > 0)
{
lpitchangle = lcurrentpangle - gimf;
}
if(lcurrentpangle - lpitchangle < 0)
{
lpitchangle = lcurrentpangle + gimf;
}
if(rcurrentpangle - rpitchangle > 0)
{
rpitchangle = rcurrentpangle - gimf;
}
if(rcurrentpangle - rpitchangle < 0)
{
rpitchangle = rcurrentpangle + gimf;
}
if(lcurrentyangle - lyawangle > 0)
{
lyawangle = lcurrentyangle - gimf;
}
if(lcurrentyangle - lyawangle < 0)
{
lyawangle = lcurrentyangle + gimf;
}
if(rcurrentyangle - ryawangle > 0)
{
ryawangle = rcurrentyangle - gimf;
}
if(rcurrentyangle - ryawangle < 0)
{
ryawangle = rcurrentyangle + gimf;
}
SetThrusterDir(th_main[0],_V(sin(lyawangle)*cos(lpitchangle),-sin(lpitchangle),cos(lyawangle)*cos(lpitchangle)));//left
SetThrusterDir(th_main[1],_V(sin(ryawangle)*cos(rpitchangle),-sin(rpitchangle),cos(ryawangle)*cos(rpitchangle)));//right
}
else
{
SetThrusterDir(th_main[0],_V(0,0,1));
SetThrusterDir(th_main[1],_V(0,0,1));
}
lcurrentpangle = lpitchangle;
rcurrentpangle = rpitchangle;
lcurrentyangle = lyawangle;
rcurrentyangle = ryawangle;
}
}