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 DockingProbe::TimeStep(double simt, double simdt)
{
if (!FirstTimeStepDone) {
DoFirstTimeStep();
FirstTimeStepDone = true;
return;
}
if (UndockNextTimestep) {
UpdatePort(Dockparam[1] * 0.5, simdt);
OurVessel->Undock(ourPort);
UndockNextTimestep = false;
}
if (ExtendingRetracting > 0) {
if (Status >= DOCKINGPROBE_STATUS_EXTENDED) {
Status = DOCKINGPROBE_STATUS_EXTENDED;
ExtendingRetracting = 0;
Dockproc = DOCKINGPROBE_PROC_UNDOCKED;
OurVessel->Undocking(ourPort);
OurVessel->SetDockingProbeMesh();
} else {
Status += 0.33 * simdt;
}
} else if (ExtendingRetracting < 0) {
if (Status <= DOCKINGPROBE_STATUS_RETRACTED) {
Status = DOCKINGPROBE_STATUS_RETRACTED;
ExtendingRetracting = 0;
OurVessel->HaveHardDocked(ourPort);
OurVessel->SetDockingProbeMesh();
} else {
Status -= 0.33 * simdt;
}
}
if (Dockproc == DOCKINGPROBE_PROC_SOFTDOCKED) {
UpdatePort(Dockparam[1] * 0.5, simdt);
Dockproc = DOCKINGPROBE_PROC_HARDDOCKED;
} else if (Dockproc == DOCKINGPROBE_PROC_HARDDOCKED) {
if (Status > DOCKINGPROBE_STATUS_RETRACTED) {
UpdatePort(Dockparam[1] * 0.5 * Status / 0.9, simdt);
} else {
UpdatePort(_V(0,0,0), simdt);
Dockproc = DOCKINGPROBE_PROC_UNDOCKED;
}
}
// sprintf(oapiDebugString(), "Docked %d Status %.3f Dockproc %d ExtendingRetracting %d", (Docked ? 1 : 0), Status, Dockproc, ExtendingRetracting);
// Switching logic
if (OurVessel->DockingProbeExtdRelSwitch.IsUp() && IsPowered()) {
Extend();
} else if (OurVessel->DockingProbeExtdRelSwitch.IsDown()) {
if ((!OurVessel->DockingProbeRetractPrimSwitch.IsCenter() && OurVessel->DockProbeMnACircuitBraker.IsPowered() && OurVessel->PyroBusA.Voltage() > SP_MIN_DCVOLTAGE) ||
(!OurVessel->DockingProbeRetractSecSwitch.IsCenter() && OurVessel->DockProbeMnBCircuitBraker.IsPowered() && OurVessel->PyroBusB.Voltage() > SP_MIN_DCVOLTAGE)) {
int ActiveCharges = 0;
if (OurVessel->DockingProbeRetractPrimSwitch.IsUp()) ActiveCharges = ActiveCharges | DOCKINGPROBE_CHARGE_PRIM1;
if (OurVessel->DockingProbeRetractPrimSwitch.IsDown()) ActiveCharges = ActiveCharges | DOCKINGPROBE_CHARGE_PRIM2;
if (OurVessel->DockingProbeRetractSecSwitch.IsUp()) ActiveCharges = ActiveCharges | DOCKINGPROBE_CHARGE_SEC1;
if (OurVessel->DockingProbeRetractSecSwitch.IsDown()) ActiveCharges = ActiveCharges | DOCKINGPROBE_CHARGE_SEC2;
if ((ActiveCharges & RetractChargesUsed)!= ActiveCharges) Retract();
RetractChargesUsed = RetractChargesUsed | ActiveCharges;
// sprintf(oapiDebugString(), "Charge Used: P1%d P2%d S1%d S2%d", RetractChargesUsed & DOCKINGPROBE_CHARGE_PRIM1 , RetractChargesUsed & DOCKINGPROBE_CHARGE_PRIM2 , RetractChargesUsed & DOCKINGPROBE_CHARGE_SEC1 , RetractChargesUsed & DOCKINGPROBE_CHARGE_SEC2);
}
}
///
/// Begin Advanced Docking Code
///
if (DockingMethod > ADVANCED){
// Code that follows is largely lifted from Atlantis...
// Goal is to handle close proximity docking between a probe and drogue
VECTOR3 gdrgPos, gdrgDir, gprbPos, gprbDir, gvslPos, rvel, pos, dir, rot;
OurVessel->Local2Global (Dockparam[0],gprbPos); //converts probe location to global
OurVessel->GlobalRot (Dockparam[1],gprbDir); //rotates probe direction to global
// Search the complete vessel list for a grappling candidate.
// Not very scalable ...
for (DWORD i = 0; i < oapiGetVesselCount(); i++) {
OBJHANDLE hV = oapiGetVesselByIndex (i);
if (hV == OurVessel->GetHandle()) continue; // we don't want to grapple ourselves ...
oapiGetGlobalPos (hV, &gvslPos);
if (dist (gvslPos, gprbPos) < oapiGetSize (hV)) { // in range
VESSEL *v = oapiGetVesselInterface (hV);
DWORD nAttach = v->AttachmentCount (true);
for (DWORD j = 0; j < nAttach; j++) { // now scan all attachment points of the candidate
ATTACHMENTHANDLE hAtt = v->GetAttachmentHandle (true, j);
const char *id = v->GetAttachmentId (hAtt);
if (strncmp (id, "PADROGUE", 8)) continue; // attachment point not compatible
v->GetAttachmentParams (hAtt, pos, dir, rot);
v->Local2Global (pos, gdrgPos); // converts found drogue position to global
v->GlobalRot (dir, gdrgDir); // rotates found drogue direction to global
if (dist (gdrgPos, gprbPos) < COLLISION_DETECT_RANGE && DockingMethod == ADVANCEDPHYSICS) { // found one less than a meter away!
// Detect if collision has happend, if so, t will return intersection point along the probe line X(t) = gprbPos + t * gprbDir
double t = CollisionDetection(gprbPos, gprbDir, gdrgPos, gdrgDir);
// Calculate time of penetration according to current velocity
OurVessel->GetRelativeVel(hV, rvel);
// Determine resultant force
//APPLY rforce to DockingProbe Vessel, and APPLY -rforce to Drogue Vessel
return;
}
if (dist(gdrgPos, gprbPos) < CAPTURE_DETECT_RANGE && DockingMethod > ADVANCED) {
// If we're within capture range, set docking port to attachment so docking can take place
// Originally, I would have used the Attachment features to soft dock and move the LM during retract
// but Artlav's docking method does this better and uses the docking port itself.
// Attachment is being used as a placeholder for the docking port and to identify its orientation.
OurVessel->GetAttachmentParams(hattPROBE, pos, dir, rot);
DOCKHANDLE dock = OurVessel->GetDockHandle(ourPort);
OurVessel->SetDockParams(dock, pos, dir, rot);
}
}//for nAttach
}//if inRange
}//for nVessel
}
}
