// Finish setup
void Atlantis_SRB::clbkPostCreation ()
{
// find out which side of the ET we are attached to
OBJHANDLE hTank = GetDockStatus (GetDockHandle (0));
if (hTank) {
for (UINT i = 1; i <= 2; i++) {
OBJHANDLE hSRB = oapiGetDockStatus (oapiGetDockHandle (hTank, i));
if (hSRB == GetHandle()) {
srbpos = (i == 1 ? SRB_LEFT : SRB_RIGHT);
break;
}
}
}
if (srbpos != SRB_UNDEFINED)
SetThrusterDir(th_bolt, srbpos == SRB_LEFT ? _V(-0.4,-0.9165,0) : _V(-0.4,0.9165,0));
}
void Saturn1b::AttitudeLaunch1()
{
//Original code function by Richard Craig From MErcury Sample by Rob CONLEY
// Modification for NASSP specific needs by JL Rocca-Serra
VECTOR3 ang_vel;
GetAngularVel(ang_vel);// gets current angular velocity for stabilizer and rate control
// variables to store each component deflection vector
VECTOR3 rollvectorl={0.0,0.0,0.0};
VECTOR3 rollvectorr={0.0,0.0,0.0};
VECTOR3 pitchvector={0.0,0.0,0.0};
VECTOR3 yawvector={0.0,0.0,0.0};
VECTOR3 yawvectorm={0.0,0.0,0.0};
VECTOR3 pitchvectorm={0.0,0.0,0.0};
//************************************************************
// variables to store Manual control levels for each axis
double tempP = 0.0;
double tempY = 0.0;
double tempR = 0.0;
//************************************************************
// Variables to store correction factors for rate control
double rollcorrect = 0.0;
double yawcorrect= 0.0;
double pitchcorrect = 0.0;
//************************************************************
// gets manual control levels in each axis, this code copied directly from Rob Conley's Mercury Atlas
if (autopilot) {
tempR = AtempR ;
tempP = AtempP ;
tempY = AtempY ;
} else {
tempP = GetManualControlLevel(THGROUP_ATT_PITCHDOWN, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE) - GetManualControlLevel(THGROUP_ATT_PITCHUP, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE);
tempY = GetManualControlLevel(THGROUP_ATT_YAWLEFT, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE) - GetManualControlLevel(THGROUP_ATT_YAWRIGHT, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE);
tempR = GetManualControlLevel(THGROUP_ATT_BANKLEFT, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE) - GetManualControlLevel(THGROUP_ATT_BANKRIGHT, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE);
}
//sprintf (oapiDebugString(), "roll input: %f, roll vel: %f,pitch input: %f, pitch vel: %f", tempR, ang_vel.z,tempP, ang_vel.x);
//*************************************************************
//Creates correction factors for rate control in each axis as a function of input level
// and current angular velocity. Varies from 1 to 0 as angular velocity approaches command level
// multiplied by maximum rate desired
if(tempR != 0.0) {
rollcorrect = (1/(fabs(tempR)*0.275))*((fabs(tempR)*0.275)-fabs(ang_vel.z));
if((tempR > 0 && ang_vel.z > 0) || (tempR < 0 && ang_vel.z < 0)) {
rollcorrect = 1;
}
}
if(tempP != 0.0) {
pitchcorrect = (1/(fabs(tempP)*0.175))*((fabs(tempP)*0.175)-fabs(ang_vel.x));
if((tempP > 0 && ang_vel.x > 0) || (tempP < 0 && ang_vel.x < 0)) {
pitchcorrect = 1;
}
}
if(tempY != 0.0) {
yawcorrect = (1/(fabs(tempY)*0.175))*((fabs(tempY)*0.175)-fabs(ang_vel.y));
if((tempY > 0 && ang_vel.y < 0) || (tempY < 0 && ang_vel.y > 0)) {
yawcorrect = 1;
}
}
//*************************************************************
// Create deflection vectors in each axis
pitchvector = _V(0.0,0.05*tempP*pitchcorrect,0.0);
pitchvectorm = _V(0.0,0.09*tempP*pitchcorrect,0.0);
yawvector = _V(0.05*tempY*yawcorrect,0.0,0.0);
yawvectorm = _V(0.09*tempY*yawcorrect,0.0,0.0);
rollvectorl = _V(0.0,0.60*tempR*rollcorrect,0.0);
rollvectorr = _V(0.0,-0.60*tempR*rollcorrect,0.0);
//*************************************************************
// create opposite vectors for "gyro stabilization" if command levels are 0
if(tempP==0.0) {
pitchvectorm=_V(0.0,0.8*ang_vel.x*3,0.0);
}
if(tempY==0.0) {
yawvectorm=_V(-0.8*ang_vel.y*3,0.0,0.0);
}
if(tempR==0.0) {
rollvectorl = _V(0.0,0.95*ang_vel.z*3,0.0);
}
//**************************************************************
// Sets thrust vectors by simply adding up all the axis deflection vectors and the
// "neutral" default vector
// Only the four outer engines are gimbaled
SetThrusterDir(th_main[0],pitchvectorm+yawvectorm+rollvectorl+_V( 0,0,1));
SetThrusterDir(th_main[1],pitchvectorm+yawvectorm+_V( 0,0,1));
SetThrusterDir(th_main[2],pitchvectorm+yawvectorm-rollvectorl+_V( 0,0,1));
SetThrusterDir(th_main[3],pitchvectorm+yawvectorm+_V( 0,0,1));
// sprintf (oapiDebugString(), "pitch vector: %f, roll vel: %f", tempR, ang_vel.z);
}
void Atlantis_SRB::SetThrustGimbal (const VECTOR3 &dir)
{
SetThrusterDir (th_main, dir);
}
void Saturn1b::AttitudeLaunch4()
{
VECTOR3 ang_vel;
GetAngularVel(ang_vel);// gets current angular velocity for stabilizer and rate control
// variables to store each component deflection vector
VECTOR3 rollvectorl={0.0,0.0,0.0};
VECTOR3 rollvectorr={0.0,0.0,0.0};
VECTOR3 pitchvector={0.0,0.0,0.0};
VECTOR3 yawvector={0.0,0.0,0.0};
VECTOR3 yawvectorm={0.0,0.0,0.0};
VECTOR3 pitchvectorm={0.0,0.0,0.0};
//************************************************************
// variables to store Manual control levels for each axis
double tempP = 0.0;
double tempY = 0.0;
double tempR = 0.0;
//************************************************************
// Variables to store correction factors for rate control
double rollcorrect = 0.0;
double yawcorrect= 0.0;
double pitchcorrect = 0.0;
//************************************************************
// gets manual control levels in each axis, this code copied directly from Rob Conley's Mercury Atlas
tempP = GetManualControlLevel(THGROUP_ATT_PITCHDOWN, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE) - GetManualControlLevel(THGROUP_ATT_PITCHUP, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE);
tempY = GetManualControlLevel(THGROUP_ATT_YAWLEFT, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE) - GetManualControlLevel(THGROUP_ATT_YAWRIGHT, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE);
tempR = GetManualControlLevel(THGROUP_ATT_BANKLEFT, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE) - GetManualControlLevel(THGROUP_ATT_BANKRIGHT, MANCTRL_ANYDEVICE, MANCTRL_ANYMODE);
//*************************************************************
//Creates correction factors for rate control in each axis as a function of input level
// and current angular velocity. Varies from 1 to 0 as angular velocity approaches command level
// multiplied by maximum rate desired
if(tempR != 0.0) {
rollcorrect = (1/(fabs(tempR)*0.35))*((fabs(tempR)*0.35)-fabs(ang_vel.z));
}
if(tempP != 0.0) {
pitchcorrect = (1/(fabs(tempP)*0.175))*((fabs(tempP)*0.175)-fabs(ang_vel.x));
if((tempP > 0 && ang_vel.x > 0) || (tempP < 0 && ang_vel.x < 0)) {
pitchcorrect = 1;
}
}
if(tempY != 0.0) {
yawcorrect = (1/(fabs(tempY)*0.175))*((fabs(tempY)*0.175)-fabs(ang_vel.y));
if((tempY > 0 && ang_vel.y < 0) || (tempY < 0 && ang_vel.y > 0)) {
yawcorrect = 1;
}
}
//*************************************************************
// Create deflection vectors in each axis
pitchvector = _V(0.0,0.05*tempP*pitchcorrect,0.0);
pitchvectorm = _V(0.0,0.09*tempP*pitchcorrect,0.0);
yawvector = _V(0.05*tempY*yawcorrect,0.0,0.0);
yawvectorm = _V(0.09*tempY*yawcorrect,0.0,0.0);
rollvectorl = _V(0.0,0.090*tempR*rollcorrect,0.0);
rollvectorr = _V(0.0,-0.09*tempR*rollcorrect,0.0);
//*************************************************************
// create opposite vectors for "gyro stabilization" if command levels are 0
if(tempP==0.0) {
pitchvectorm=_V(0.0,0.95*ang_vel.x*2,0.0);
}
if(tempY==0.0) {
yawvectorm=_V(-0.95*ang_vel.y*2,0.0,0.0);
}
if(tempR==0.0) {
rollvectorl = _V(0.0,0.99*ang_vel.z*2,0.0);
}
//**************************************************************
// Sets thrust vectors by simply adding up all the axis deflection vectors and the
// "neutral" default vector
SetThrusterDir(th_main[0],pitchvectorm+yawvectorm+_V( 0,0,1));//4
// sprintf (oapiDebugString(), "roll input: %f, roll vel: %f", tempR, ang_vel.z);
}
