// Simulation time step
void Atlantis_SRB::clbkPostStep (double simt, double simdt, double mjd)
{
double lvl = 0.0;
double met = 0.0;
if (bMainEngine) {
met = simt-t0;
lvl = ThrustProfile (met);
SetThrusterLevel (th_main, lvl);
}
if (bSeparationEngine) {
if (simt-tsep > 0.5) {
DelThruster (th_bolt);
bSeparationEngine = false;
}
}
if (bGimbalCmd) {
double dg_max = simdt*SRB_GIMBAL_SPEED;
VECTOR3 gimbalcur = GetThrustGimbal();
VECTOR3 gdiff = gimbalcmd-gimbalcur;
double dg = length(gdiff);
if (dg > dg_max) {
gdiff *= dg_max/dg;
} else {
bGimbalCmd = false;
}
SetThrustGimbal (gimbalcur + gdiff);
}
#ifdef UNDEF
extern void GetSRB_State (double, double&, double&);
//sprintf (oapiDebugString(), "SRB mass = %f", GetMass());
if (bMainEngine) {
double met = simt-t0;
if (met >= SRB_CUTOUT_TIME) {
SetThrusterLevel (th_main, 0);
bMainEngine = false;
} else {
double thrust_level, prop_level;
GetSRB_State (met, thrust_level, prop_level);
SetThrusterLevel (th_main, thrust_level);
}
if (bSeparationEngine) {
static double bolt_t = 0.5;
double srb_dt = simt - srb_separation_time;
if (srb_dt > bolt_t) {
DelThruster (th_bolt);
bSeparationEngine = false;
} else {
SetThrusterLevel (th_bolt, sqrt (1.0-srb_dt/bolt_t));
}
}
}
//if (GetAltitude() < 0.0) oapiDeleteVessel (GetHandle());
#endif
}
bool Atlantis_SRB::Ignite ()
{
if (GetPropellantMass (ph_main) == SRB_MAX_PROPELLANT_MASS) {
SetThrusterLevel (th_main, 1.0);
t0 = oapiGetSimTime();
bMainEngine = true;
if (launchelev) {
SetTouchdownPoints (tdvtx, ntdvtx); // reset touchdown points
SetSize (23.0);
}
return true;
}
return false;
}
void Atlantis_SRB::FireBolt ()
{
SetThrusterLevel (th_bolt, 1.0);
tsep = oapiGetSimTime();
bSeparationEngine = true;
}
void Saturn1b::AutoPilot(double autoT)
{
TRACESETUP("Saturn1b::AutoPilot");
const double GRAVITY=6.67259e-11;
static int first_time=1;
static int t = 0;
static int out_level=0;
double level=0.;
double altitude;
double pitch;
double pitch_c;
double heading;
double bank;
VECTOR3 rhoriz;
double TO_HDG = agc.GetDesiredAzimuth();
AltitudePREV = altitude = GetAltitude();
VESSELSTATUS vsp;
GetStatus(vsp);
double totalRot=0;
totalRot=vsp.vrot.x+vsp.vrot.y+vsp.vrot.z;
if (fabs(totalRot) >= 0.0025){
StopRot = true;
}
// This vector rotation will be used to tell if heads up (rhoriz.z<0) or heads down.
HorizonRot(_V(1,0,0), rhoriz);
//
// Shut down the engines when we reach the desired
// orbit.
//
double apogee, perigee;
OBJHANDLE ref = GetGravityRef();
GetApDist(apogee);
GetPeDist(perigee);
apogee = (apogee - oapiGetSize(ref)) / 1000.;
perigee = (perigee - oapiGetSize(ref)) / 1000.;
// We're aiming for periapsis and shutdown when apoapsis is reached at the opposite side of the orbit
if (apogee >= agc.GetDesiredApogee() && perigee >= agc.GetDesiredPerigee() - 0.1) {
// See Saturn::CheckForLaunchShutdown()
if (GetThrusterLevel(th_main[0]) > 0){
SetThrusterLevel(th_main[0], 0);
if (oapiGetTimeAcceleration() > 1.0)
oapiSetTimeAcceleration(1.0);
agc.LaunchShutdown();
// Reset autopilot commands
AtempP = 0;
AtempY = 0;
AtempR = 0;
}
return;
}
// navigation
pitch = GetPitch();
pitch = pitch*180./PI;
//sprintf(oapiDebugString(), "Autopilot %f", altitude);
// guidance
pitch_c = GetCPitch(autoT);
// control
if (altitude > 4500) {
// Damp roll motion
bank = GetBank();
bank = bank *180. / PI;
if (bank > 90) bank = bank - 180.;
else if (bank < -90) bank = bank + 180.;
AtempR = -bank / 20.0;
if (fabs(bank) < 0.3) AtempR = 0;
// navigation
pitch = GetPitch();
pitch = pitch * 180. / PI;
if (IGMEnabled) {
VECTOR3 target;
double pit, yaw;
double bradius = oapiGetSize(ref);
double bmass = oapiGetMass(ref);
double mu = GRAVITY * bmass;
// Aim for periapsis
double altco = agc.GetDesiredPerigee() * 1000.;
double velo = sqrt(mu / (bradius + altco));
target.x = velo;
target.y = 0.0;
target.z = altco;
LinearGuidance(target, pit, yaw);
AtempP=(pit * DEG - pitch) / 30.0;
if (AtempP < -0.15) AtempP = -0.15;
if (AtempP > 0.15) AtempP = 0.15;
}
else {
// guidance
pitch_c = GetCPitch(autoT);
// control
double SatApo;
GetApDist(SatApo);
if ((SatApo >= ((agc.GetDesiredApogee() *.90) + ERADIUS)*1000) || MissionTime >= IGMStartTime)
IGMEnabled = true;
level = pitch_c - pitch;
//sprintf(oapiDebugString(), "Autopilot Pitch Mode%f", elemSaturn1B.a );
if (fabs(level)<10 && StopRot){ // above atmosphere, soft correction
AtempP = 0.0;
AtempR = 0.0;
AtempY = 0.0;
StopRot = false;
}
if (fabs(level)<0.05){ // above atmosphere, soft correction
AtempP = 0.0;
AtempR = 0.0;
AtempY = 0.0;
}
else if (level>0 && fabs(vsp.vrot.z) < 0.09){
AtempP = -(fabs(level) / 10.);
if (AtempP < -1.0)AtempP = -1.0;
if (rhoriz.z>0) AtempP = -AtempP;
}
else if (level<0 && fabs(vsp.vrot.z) < 0.09) {
AtempP = (fabs(level) / 10.);
if (AtempP > 1.0) AtempP = 1.0;
if (rhoriz.z>0) AtempP = -AtempP;
}
else {
AtempP = 0.0;
AtempR = 0.0;
AtempY = 0.0;
}
// sprintf(oapiDebugString(), "autoT %f AtempP %f AtempR %f AtempY %f altitude %f pitch %f pitch_c %f",
// autoT, AtempP, AtempR, AtempY, altitude, pitch, pitch_c);
}
}
// sprintf(oapiDebugString(), "Alt %f Pitch %f Roll %f Yaw %f autoT %f", altitude, AtempP, AtempR, AtempY, autoT);
double slip;
VECTOR3 az;
VECTOR3 up, north, east, ygl, zgl, zerogl;
OBJHANDLE hbody=GetGravityRef();
double bradius=oapiGetSize(hbody);
// set up our reference frame
Local2Global(_V(0.0, 0.0, 0.0), zerogl);
Local2Global(_V(0.0, 1.0, 0.0), ygl);
Local2Global(_V(0.0, 0.0, 1.0), zgl);
ygl=ygl-zerogl;
zgl=zgl-zerogl;
oapiGetHeading(GetHandle(),&heading);
heading = heading*180./PI;
// Inclination control
static int incinit = 0;
static ELEMENTS elemlast;
static double incratelast;
ELEMENTS elem;
GetElements(ref, elem, 0, 0, FRAME_EQU);
double incrate = (elem.i - elemlast.i) / oapiGetSimStep();
double incraterate = (incrate - incratelast) / oapiGetSimStep();
double target = (agc.GetDesiredInclination() - elem.i * DEG) / (FirstStageShutdownTime - MissionTime);
if (agc.GetDesiredInclination() != 0 && autoT > 45) {
if (incinit < 2) {
incinit++;
AtempY = 0;
} else {
if (autoT < FirstStageShutdownTime - 10) {
AtempY = (incrate * DEG - target) / 0.7 + incraterate * DEG / 2.;
if (AtempY < -0.1) AtempY = -0.1;
if (AtempY > 0.1) AtempY = 0.1;
} else if (autoT < FirstStageShutdownTime + 10) {
AtempY = 0;
} else {
AtempY = (elem.i * DEG - agc.GetDesiredInclination()) / 7. + (incrate * DEG ) / 1.;
if (AtempY < -0.01) AtempY = -0.01;
if (AtempY > 0.01) AtempY = 0.01;
}
}
}
elemlast = elem;
incratelast = incrate;
// stage handling
switch (stage){
case LAUNCH_STAGE_ONE:
GetRelativePos(hbody, up);
up=Normalize(up);
agc.EquToRel(PI/2.0, 0.0, bradius, north);
north=Normalize(north);
east=Normalize(CrossProduct(north, up));
north=Normalize(CrossProduct(up, east));
az=east*sin(TO_HDG*RAD)-north*cos(TO_HDG*RAD);
if(autoT < 60.0) normal=Normalize(CrossProduct(up, az));
slip=GetSlipAngle()*DEG;
if(autoT < 10.) {
AtempR=0.0;
AtempY=0.0;
// cancel out the yaw maneuver...
AtempY=(-0.4+asin(zgl*normal)*DEG)/20.0;
}
if(autoT > 10.0 && autoT < 30.0) {
// roll program
AtempR=asin(ygl*normal)*DEG/20.0;
AtempY=asin(zgl*normal)*DEG/20.0;
if (AtempR < -0.25) AtempR = -0.25;
if (AtempR > 0.25) AtempR = 0.25;
}
if(autoT > 30.0 && autoT < 45.0) {
//pitch and adjust for relative wind
AtempR=asin(ygl*normal)*DEG/20.0;
//AtempY=(slip+asin(zgl*normal)*DEG)/20.0;
AtempY=(TO_HDG-(heading+slip))/20.0;
if (AtempR < -0.25) AtempR = -0.25;
if (AtempR > 0.25) AtempR = 0.25;
}
pitch = GetPitch();
pitch=pitch*180./PI;
pitch_c=GetCPitch(autoT);
AtempP = (pitch_c - pitch);
// Fix for LC 39
if (autoT < 10 && heading > 180)
AtempP = -(180. - pitch_c - pitch);
if (AtempP > 1.0) AtempP = 1.0;
if (AtempP < -1.0) AtempP = -1.0;
// zero angle-of-attack...
if(autoT > 45.0 && autoT < 115.0) {
/// \todo Disabled for now, the Saturn 1B doesn't seem to do that...
//double aoa=GetAOA()*DEG;
//pitch_c=pitch+aoa-0.3;
AtempP=(pitch_c - pitch) / 5.0;
if(AtempP < -0.2) AtempP = -0.2;
if(AtempP > 0.2) AtempP = 0.2;
// sprintf(oapiDebugString(), " pitch=%.3f pc=%.3f ap=%.3f", pitch, pitch_c, AtempP);
}
if (autoT > 115.0) {
if (autoT < 120.0) {
if (AtempP < -0.1) AtempP = -0.1;
if (AtempP > 0.1) AtempP = 0.1;
} else {
if (AtempP < -0.2) AtempP = -0.2;
if (AtempP > 0.2) AtempP = 0.2;
}
normal=Normalize(CrossProduct(Normalize(vsp.rpos), Normalize(vsp.rvel)));
}
// sprintf(oapiDebugString(), "roll=%.3f yaw=%.3f slip=%.3f sum=%.3f hdg+slip=%.3f hdg=%.3f ay=%.3f",
// asin(ygl*normal)*DEG, asin(zgl*normal)*DEG, slip, slip+asin(zgl*normal)*DEG, heading+slip, heading, AtempY);
// sprintf(oapiDebugString(), "autoT %f AtempP %f AtempR %f AtempY %f altitude %f pitch %f pitch_c %f rhoriz.z %f",
// autoT, AtempP, AtempR, AtempY, altitude, pitch, pitch_c, rhoriz.z);
/*
char buffer[80];
sprintf(buffer,"AtempP %f AtempR %f AtempY %f", AtempP, AtempR, AtempY);
TRACE(buffer);
*/
AttitudeLaunch1();
break;
case LAUNCH_STAGE_SIVB:
AttitudeLaunchSIVB();
break;
}
// sprintf(oapiDebugString(), "AP - inc %f rate %f target %f raterate %f AtempP %f AtempR %f AtempY %f", elem.i * DEG, incrate * DEG, target, incraterate * DEG, AtempP, AtempR, AtempY);
// sprintf(oapiDebugString(), "AP - pitch %f pitch_c %f heading %f AtempP %f AtempR %f AtempY %f", pitch, pitch_c, heading, AtempP, AtempR, AtempY);
}