예제 #1
0
// 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
}
예제 #2
0
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;
}
예제 #3
0
void Atlantis_SRB::FireBolt ()
{
	SetThrusterLevel (th_bolt, 1.0);
	tsep = oapiGetSimTime();
	bSeparationEngine = true;
}
예제 #4
0
파일: sat1ap.cpp 프로젝트: dseagrav/NASSP
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);
}