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 Dragonfly::Timestep (double simt)
{
VECTOR3 cg;
VECTOR3 dist,pos;
double dlevel, ratio;
float line;
static int vern;
static int intr;
static int rotmode;
// ******************** Set linear, rotation, disabled RCS mode
if (NAV_handle<3)
{SetAttitudeMode(NAV_handle+1);
NAV_handle=3;
};
// ******************** Kill rot mode can be called via a CB
//sprintf(oapiDebugString(),"%0.4f %0.4f %0.4f",Internals.Valves[23]->Temp,Internals.Tanks[11]->Temp,Internals.Tanks[12]->Temp);
if (Kill_rot==1)
{ if (killset==0) { killset=1; //we've set the killrot
rotmode= Internals.Nav_mode_switch->pos;
Internals.Nav_mode_switch->pos=0;//go to rot mode
oapiTriggerPanelRedrawArea(0,Internals.Nav_mode_switch->idx);//and redraw it
vern=VERN_handle;
intr=INTR_handle;
if (Manual_RCS) {
VERN_handle=1;//no vernier
Internals.Vern_mode_switch->pos=1;//no vernier
oapiTriggerPanelRedrawArea(0,Internals.Vern_mode_switch->idx);
INTR_handle=1;//normal mode
Internals.Intr_mode_switch->pos=1;
oapiTriggerPanelRedrawArea(0,Internals.Intr_mode_switch->idx);
SetNormalRCS(); //go to auto mode
};
SetAttitudeMode(1);//go to rot mode
ActivateNavmode(NAVMODE_KILLROT); //stop all rotation;
}//end of killset
if (killset==1){ //we are killrotting, monitoring the end of it
if (GetNavmodeState(NAVMODE_KILLROT)==0)//no longer killrotting
{//go back to previously saved RCS mode
Internals.Nav_mode_switch->pos=rotmode;
oapiTriggerPanelRedrawArea(0,Internals.Nav_mode_switch->idx);
SetAttitudeMode(rotmode+1);
Internals.Vern_mode_switch->pos=vern;
VERN_handle=vern;
oapiTriggerPanelRedrawArea(0,Internals.Vern_mode_switch->idx);
Internals.Intr_mode_switch->pos=intr;
INTR_handle=intr;
oapiTriggerPanelRedrawArea(0,Internals.Intr_mode_switch->idx);
//then inform the loop that killrot has ended
Kill_rot=0;
killset=0;
};
}//end of killset
}; //end of killrot=0;
if ((killset)&&(Kill_rot==0)) //we've been asked to stop killrot
{//go back to previously saved RCS mode
Internals.Nav_mode_switch->pos=rotmode;
oapiTriggerPanelRedrawArea(0,Internals.Nav_mode_switch->idx);
SetAttitudeMode(rotmode+1);
Internals.Vern_mode_switch->pos=vern;
VERN_handle=vern;
oapiTriggerPanelRedrawArea(0,Internals.Vern_mode_switch->idx);
Internals.Intr_mode_switch->pos=intr;
INTR_handle=intr;
oapiTriggerPanelRedrawArea(0,Internals.Intr_mode_switch->idx);
DeactivateNavmode(NAVMODE_KILLROT);
Kill_rot=0; //not doing anything
killset=0;
};
//********************* Handle the vessel animations. Docking latch mecanism and the 2 antenas
if (Dock_target_object) {
//Dock_target_object=oapiGetVesselByIndex(1);
oapiGetGlobalPos(Dock_target_object,&dist);
Global2Local(dist,pos);//now we have a position w.r.t ship
line=_vector3(pos.x,pos.y,pos.z).mod();
UP_trg=1-acos(pos.y/line)/acos(-1)*180.0/75.0;
UY_trg=atan2(pos.x/line,pos.z/line);
if (UY_trg>acos(-1)) UY_trg-=2*acos(-1);
UY_trg=0.5-UY_trg/acos(-1)*180.0/300.0;
LP_trg=acos(pos.y/line)/acos(-1)*180.0/75.0-1;
LY_trg=UY_trg;//same azimuth;
if (UAnt_handle==-1) {//Aquire & Track
if (UY_trg<UY_pos) UY_handle=-1;
if (UY_trg>UY_pos) UY_handle=1;
if (UP_trg<UP_pos) UP_handle=-1;
if (UP_trg>UP_pos) UP_handle=1;
};
if (LAnt_handle==-1) {//Aquire & Track
if (LY_trg<LY_pos) LY_handle=-1;
if (LY_trg>LY_pos) LY_handle=1;
if (LP_trg<LP_pos) LP_handle=-1;
if (LP_trg>LP_pos) LP_handle=1;
};
LAnt_SStr=1.0-(fabs(LY_trg-LY_pos)+fabs(LP_trg-LP_pos))/0.04;
UAnt_SStr=1.0-(fabs(UY_trg-UY_pos)+fabs(UP_trg-UP_pos))/0.04;
};
if (UAnt_handle==1) //bring upper antena home
{
if (UP_pos>0) UP_handle=-1;
else UP_handle=0;
if (UY_pos>0.51) UY_handle=-1;
else if (UY_pos<0.49) UY_handle=1;
else {UY_pos=0.5;;UY_handle=0;}
};
if (LAnt_handle==1) //bring lower antena home
{ if (LP_pos>0) LP_handle=-1;
else LP_handle=0;
if (LY_pos>0.51) LY_handle=-1;
else if (LY_pos<0.49) LY_handle=1;
else {LY_pos=0.5;;LY_handle=0;}
};
//stop all motion
if (UAnt_handle==0)
{UP_handle=0; UY_handle=0; UAnt_handle=2;};
if (LAnt_handle==0)
{LP_handle=0; LY_handle=0; LAnt_handle=2;};
if (latch_handle)
{ if ((latch_handle<0)&&(dock_latched>0)) dock_latched-=oapiGetSysStep()/10.0;
if ((latch_handle>0)&&(dock_latched<1)) dock_latched+=oapiGetSysStep()/10.0;
if (dock_latched<0) dock_latched=0;
if(dock_latched>1) dock_latched=1;
SetAnimState (anim_latch, dock_latched);
}
if ((*AC_power>0)&&(UY_handle))
{if ((UY_handle<0)&&(UY_pos>0)) UY_pos-=oapiGetSysStep()/18.0;
if ((UY_handle>0)&&(UY_pos<1)) UY_pos+=oapiGetSysStep()/18.0;
if (UY_pos<0) UY_pos=0;
if (UY_pos>1) UY_pos=1;
SetAnimState (anim_UY_ant, UY_pos);
float ang=(150-UY_pos*300.0)/180.0*acos(-1);
Upper_ant_pitch.trans.P.rotparam.axis=_V(cos(ang),0,-sin(ang));
};
if ((*AC_power>0)&&(UP_handle))
{if ((UP_handle<0)&&(UP_pos>0)) UP_pos-=oapiGetSysStep()/18.0;
if ((UP_handle>0)&&(UP_pos<1)) UP_pos+=oapiGetSysStep()/18.0;
if (UP_pos<0) UP_pos=0;
if (UP_pos>1) UP_pos=1;
SetAnimState (anim_UP_ant, UP_pos);
};
if ((*AC_power>0)&&(LY_handle))
{if ((LY_handle<0)&&(LY_pos>0)) LY_pos-=oapiGetSysStep()/18.0;
if ((LY_handle>0)&&(LY_pos<1)) LY_pos+=oapiGetSysStep()/18.0;
if (LY_pos<0) LY_pos=0;
if (LY_pos>1) LY_pos=1;
SetAnimState (anim_LY_ant, LY_pos);
float ang=(150-LY_pos*300.0)/180.0*acos(-1);
Lower_ant_pitch.trans.P.rotparam.axis=_V(cos(ang),0,-sin(ang));
};
if ((*AC_power>0)&&(LP_handle))
{if ((LP_handle<0)&&(LP_pos>0)) LP_pos-=oapiGetSysStep()/18.0;
if ((LP_handle>0)&&(LP_pos<1)) LP_pos+=oapiGetSysStep()/18.0;
if (LP_pos<0) LP_pos=0;
if (LP_pos>1) LP_pos=1;
SetAnimState (anim_LP_ant, LP_pos);
};
signal_flag=0;
if ((*AC_power>0)&&((UAnt_SStr>0.9)||(LAnt_SStr>0.9)))
{
sprintf(Dock_dist,"%5.0f",_vector3(pos.x,pos.y,pos.z).mod());//radar dist
VECTOR3 rel_vel;
oapiGetRelativeVel(GetHandle(),Dock_target_object,&rel_vel);
rel_vel=rel_vel+dist; //this is global;
VECTOR3 local_vel;
Global2Local(rel_vel,local_vel);//local frame vel
local_vel=local_vel-pos; //minus position is actual V vector
sprintf(Dock_vel,"%5.2f",_vector3(local_vel.x,local_vel.y,local_vel.z).mod());//total closure
sprintf(Dock_x_vel,"%3.0f",fabs(local_vel.x*100));
sprintf(Dock_y_vel,"%3.0f",fabs(local_vel.y*100));
sprintf(Dock_z_vel,"%3.0f",fabs(local_vel.z*100));
signal_flag=1;
}
//adjust for diferent RCS modes
//NAV HANDLE - 1,lin, 0,rot, -1,off
//VERN HANDLE - 1,norm, -1,vernier
//INTR HANDLE - 1,command, 0, pulse, -1, rate
if (Manual_RCS==0) //we are in auto mode
{if (VERN_handle<1) DeleteAutoRCS();// go to manual
if (INTR_handle<1) DeleteAutoRCS();// go to manual
};
double tlevel;
double tlevel_r;
static int lin_x,lin_y,lin_z;
static int rot_x,rot_y,rot_z;
if (Manual_RCS)
{//now's the tricky part
if (INTR_handle>-1) SetZeroManualRCS();//needs to be zeroed;!
if ((VERN_handle==1) &&(INTR_handle==1)) SetNormalRCS();//go to auto
else
{
switch (INTR_handle)
{
case 1: // normal command
dlevel=(VERN_handle>0?1:0.1); //vernier or not! :-) must be vernier, or this would not be manual command ...
if (Internals.Nav_mode_switch->pos==0){
if (tlevel=GetManualControlLevel(THGROUP_ATT_PITCHUP, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE)) SetThrusterGroupLevel(th_rot_up,tlevel*dlevel);
if (tlevel=GetManualControlLevel(THGROUP_ATT_PITCHDOWN, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE)) SetThrusterGroupLevel(th_rot_down,tlevel*dlevel);
if (tlevel=GetManualControlLevel(THGROUP_ATT_YAWRIGHT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE)) SetThrusterGroupLevel(th_rot_right,tlevel*dlevel);
if (tlevel=GetManualControlLevel(THGROUP_ATT_YAWLEFT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE)) SetThrusterGroupLevel(th_rot_left,tlevel*dlevel);
if (tlevel=GetManualControlLevel(THGROUP_ATT_BANKRIGHT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE)) SetThrusterGroupLevel(th_rot_rlrgt,tlevel*dlevel);
if (tlevel=GetManualControlLevel(THGROUP_ATT_BANKLEFT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE)) SetThrusterGroupLevel(th_rot_rllft,tlevel*dlevel);
} else if (Internals.Nav_mode_switch->pos==1) {
if (tlevel=GetManualControlLevel(THGROUP_ATT_BANKLEFT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) SetThrusterGroupLevel(th_lin_left,tlevel*dlevel);
if (tlevel=GetManualControlLevel(THGROUP_ATT_BANKRIGHT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) SetThrusterGroupLevel(th_lin_right,tlevel*dlevel);
if (tlevel=GetManualControlLevel(THGROUP_ATT_PITCHUP, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) SetThrusterGroupLevel(th_lin_up,tlevel*dlevel);
if (tlevel=GetManualControlLevel(THGROUP_ATT_PITCHDOWN, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) SetThrusterGroupLevel(th_lin_down,tlevel*dlevel);
if (tlevel=GetManualControlLevel(THGROUP_ATT_YAWLEFT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) SetThrusterGroupLevel(th_lin_forward,tlevel*dlevel);
if (tlevel=GetManualControlLevel(THGROUP_ATT_YAWRIGHT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) SetThrusterGroupLevel(th_lin_back,tlevel*dlevel);
}//the left/right forward/back are changed a bit.. but there is no "9" key processed
break;
case 0: //pulse type command
dlevel=(VERN_handle>0?1:0.1); //vernier or not!
if (Internals.Nav_mode_switch->pos==0){
if ((tlevel=GetManualControlLevel(THGROUP_ATT_PITCHUP, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE))&&(!rot_x++)) SetThrusterGroupLevel(th_rot_up,tlevel*dlevel);
if ((tlevel_r=GetManualControlLevel(THGROUP_ATT_PITCHDOWN, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE))&&(!rot_x++)) SetThrusterGroupLevel(th_rot_down,tlevel_r*dlevel);
if (tlevel+tlevel_r==0) rot_x=0;
if ((tlevel=GetManualControlLevel(THGROUP_ATT_YAWRIGHT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE))&&(!rot_z++)) SetThrusterGroupLevel(th_rot_right,tlevel*dlevel);
if ((tlevel_r=GetManualControlLevel(THGROUP_ATT_YAWLEFT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE))&&(!rot_z++)) SetThrusterGroupLevel(th_rot_left,tlevel_r*dlevel);
if (tlevel+tlevel_r==0) rot_z=0;
if ((tlevel=GetManualControlLevel(THGROUP_ATT_BANKRIGHT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE))&&(!rot_y++)) SetThrusterGroupLevel(th_rot_rlrgt,tlevel*dlevel);
if ((tlevel_r=GetManualControlLevel(THGROUP_ATT_BANKLEFT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE))&&(!rot_y++)) SetThrusterGroupLevel(th_rot_rllft,tlevel_r*dlevel);
if (tlevel+tlevel_r==0) rot_y=0;
} else if (Internals.Nav_mode_switch->pos==1) {
if ((tlevel=GetManualControlLevel(THGROUP_ATT_BANKLEFT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE))&&(!rot_x++)) SetThrusterGroupLevel(th_lin_left,tlevel*dlevel);
if ((tlevel_r=GetManualControlLevel(THGROUP_ATT_BANKRIGHT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE))&&(!rot_x++)) SetThrusterGroupLevel(th_lin_right,tlevel_r*dlevel);
if (tlevel+tlevel_r==0) rot_x=0;
if ((tlevel=GetManualControlLevel(THGROUP_ATT_PITCHUP, MANCTRL_ANYDEVICE, MANCTRL_LINMODE))&&(!rot_z++)) SetThrusterGroupLevel(th_lin_up,tlevel*dlevel);
if ((tlevel_r=GetManualControlLevel(THGROUP_ATT_PITCHDOWN, MANCTRL_ANYDEVICE, MANCTRL_LINMODE))&&(!rot_z++)) SetThrusterGroupLevel(th_lin_down,tlevel_r*dlevel);
if (tlevel+tlevel_r==0) rot_z=0;
if ((tlevel=GetManualControlLevel(THGROUP_ATT_YAWLEFT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE))&&(!rot_y++)) SetThrusterGroupLevel(th_lin_forward,tlevel*dlevel);
if ((tlevel_r=GetManualControlLevel(THGROUP_ATT_YAWRIGHT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE))&&(!rot_y++)) SetThrusterGroupLevel(th_lin_back,tlevel_r*dlevel);
if (tlevel+tlevel_r==0) rot_y=0;
}//the left/right forward/back are changed a bit.. but there is no "9" key processed :-(
break;
case -1: //rate command.. this is overly-complicated
dlevel=(VERN_handle>0?1:0.1); //vernier or not!
if (Internals.Nav_mode_switch->pos==0){
//pitch -up
if ((tlevel=GetManualControlLevel(THGROUP_ATT_PITCHUP, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE))&&(!rot_x++))
{
tlevel_r=GetThrusterGroupLevel(th_rot_down);
if (tlevel_r) {SetThrusterGroupLevel(th_rot_down,0.0);lin_x++;}
else IncThrusterGroupLevel(th_rot_up,tlevel*dlevel/10.0);
}
if (!tlevel) rot_x=0;
//pitch-down
if ((tlevel=GetManualControlLevel(THGROUP_ATT_PITCHDOWN, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE))&&(!lin_x++))
{
tlevel_r=GetThrusterGroupLevel(th_rot_up);
if (tlevel_r) {SetThrusterGroupLevel(th_rot_up,0.0);rot_x++;}
else IncThrusterGroupLevel(th_rot_down,tlevel*dlevel/10.0);
}
if(!tlevel) lin_x=0;
//yaw right
if ((tlevel=GetManualControlLevel(THGROUP_ATT_YAWRIGHT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE)) &&(!rot_z++))
{
tlevel_r=GetThrusterGroupLevel(th_rot_left);
if (tlevel_r){ SetThrusterGroupLevel(th_rot_left,0.0);lin_z++;}
else IncThrusterGroupLevel(th_rot_right,tlevel*dlevel/10.0);
}
if (!tlevel) rot_z=0;
//yaw right
if ((tlevel=GetManualControlLevel(THGROUP_ATT_YAWLEFT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE)) &&(!lin_z++))
{
tlevel_r=GetThrusterGroupLevel(th_rot_right);
if (tlevel_r){ SetThrusterGroupLevel(th_rot_right,0.0);rot_z++;}
else IncThrusterGroupLevel(th_rot_left,tlevel*dlevel/10.0);
}
if (!tlevel) lin_z=0;
//roll right
if ((tlevel=GetManualControlLevel(THGROUP_ATT_BANKRIGHT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE)) && (!rot_y++))
{
tlevel_r=GetThrusterGroupLevel(th_rot_rllft);
if (tlevel_r){ SetThrusterGroupLevel(th_rot_rllft,0.0);lin_y++;}
else IncThrusterGroupLevel(th_rot_rlrgt,tlevel*dlevel/10.0);
}
if (!tlevel) rot_y=0;
//roll left
if ((tlevel=GetManualControlLevel(THGROUP_ATT_BANKLEFT, MANCTRL_ANYDEVICE, MANCTRL_ROTMODE)) &&(!lin_y++))
{
tlevel_r=GetThrusterGroupLevel(th_rot_rlrgt);
if (tlevel_r){ SetThrusterGroupLevel(th_rot_rlrgt,0.0);rot_z++;}
else IncThrusterGroupLevel(th_rot_rllft,tlevel*dlevel/10.0);
}
if (!tlevel) lin_y=0;
} else if (Internals.Nav_mode_switch->pos==1) {
if ((tlevel=GetManualControlLevel(THGROUP_ATT_BANKLEFT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) &&(!rot_x++))
{
tlevel_r=GetThrusterGroupLevel(th_lin_right);
if (tlevel_r) {SetThrusterGroupLevel(th_lin_right,0.0);lin_x++;}
else IncThrusterGroupLevel(th_lin_left,tlevel*dlevel/10.0);
}
if (!tlevel) rot_x=0;
if ((tlevel=GetManualControlLevel(THGROUP_ATT_BANKRIGHT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) &&(!lin_x++))
{
tlevel_r=GetThrusterGroupLevel(th_lin_left);
if (tlevel_r) {SetThrusterGroupLevel(th_lin_left,0.0);rot_x++;}
else IncThrusterGroupLevel(th_lin_right,tlevel*dlevel/10.0);
}
if (!tlevel) lin_x=0;
if ((tlevel=GetManualControlLevel(THGROUP_ATT_PITCHUP, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) &&(!rot_z++))
{
tlevel_r=GetThrusterGroupLevel(th_lin_down);
if (tlevel_r) {SetThrusterGroupLevel(th_lin_down,0.0);lin_z++;}
else IncThrusterGroupLevel(th_lin_up,tlevel*dlevel/10.0);
}
if (!tlevel) rot_z=0;
if ((tlevel=GetManualControlLevel(THGROUP_ATT_PITCHDOWN, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) &&(!lin_z++))
{
tlevel_r=GetThrusterGroupLevel(th_lin_up);
if (tlevel_r) {SetThrusterGroupLevel(th_lin_up,0.0);rot_z++;}
else IncThrusterGroupLevel(th_lin_down,tlevel*dlevel/10.0);
}
if (!tlevel) lin_z=0;
if ((tlevel=GetManualControlLevel(THGROUP_ATT_YAWLEFT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) &&(!rot_y++))
{
tlevel_r=GetThrusterGroupLevel(th_lin_back);
if (tlevel_r) {SetThrusterGroupLevel(th_lin_back,0.0);lin_y++;}
else IncThrusterGroupLevel(th_lin_forward,tlevel*dlevel/10.0);
}
if (!tlevel) rot_y=0;
if ((tlevel=GetManualControlLevel(THGROUP_ATT_YAWRIGHT, MANCTRL_ANYDEVICE, MANCTRL_LINMODE)) && (!lin_y++))
{
tlevel_r=GetThrusterGroupLevel(th_lin_forward);
if (tlevel_r) {SetThrusterGroupLevel(th_lin_forward,0.0);rot_y++;}
else IncThrusterGroupLevel(th_lin_back,tlevel*dlevel/10.0);
}
if (!tlevel) lin_y=0;
}//the left/right forward/back are changed a bit.. but there is no "9" key processed
break;
};//end INTR switch
}//end else
}//end manual RCS
//******************************* THIS TAKES CARE OF AUTOMATIC GC MANAGEMENT...
if (!cgmode) // auto
cgofs = (GetSuperstructureCG (cg) ? cg.z : 0.0);
if (Manual_RCS==0) { //auto mode we use normal Orbiter channels
if (cgofs) {
ratio = 2.0*cgofs/(cgofs+11.1); // counter-balance level
dlevel = GetThrusterGroupLevel (THGROUP_ATT_LEFT) - GetThrusterGroupLevel (THGROUP_ATT_RIGHT);
if (dlevel > 0) IncThrusterLevel_SingleStep (th_ap[0], dlevel*ratio);
else if (dlevel < 0) IncThrusterLevel_SingleStep (th_ap[1], -dlevel*ratio);
dlevel = GetThrusterGroupLevel (THGROUP_ATT_DOWN) - GetThrusterGroupLevel (THGROUP_ATT_UP);
if (dlevel > 0) IncThrusterLevel_SingleStep (th_ap[2], dlevel*ratio);
else if (dlevel < 0) IncThrusterLevel_SingleStep (th_ap[3], -dlevel*ratio);
}}
else { //we are in manual mode, using custom thruster groups
if (cgofs) {
ratio = 2.0*cgofs/(cgofs+11.1); // counter-balance level
dlevel = GetThrusterGroupLevel (th_lin_left) - GetThrusterGroupLevel (th_lin_right);
if (dlevel > 0) IncThrusterLevel_SingleStep (th_ap[0], dlevel*ratio);
else if (dlevel < 0) IncThrusterLevel_SingleStep (th_ap[1], -dlevel*ratio);
dlevel = GetThrusterGroupLevel (th_lin_down) - GetThrusterGroupLevel (th_lin_up);
if (dlevel > 0) IncThrusterLevel_SingleStep (th_ap[2], dlevel*ratio);
else if (dlevel < 0) IncThrusterLevel_SingleStep (th_ap[3], -dlevel*ratio);
}
}
};
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);
}
