Beispiel #1
0
double AscentAP::GetTargetInclination ()
{
	double a=0.0, B=0.0;
	if (vessel->status == 0) {
		if (!launch_lat && !launch_lng) {
			double r;
			vessel->GetEquPos(launch_lng, launch_lat, r);
		}
		a = PI05-launch_lat;

		// correct launch azimuth for surface rotation
		const OBJHANDLE hRef = vessel->GetGravityRef();
		double R = oapiGetSize(hRef);           // planet mean radius
		double r = R + tgt_alt;                 // target orbit radius
		double M = oapiGetMass (hRef);          // reference body mass
		double v0 = sqrt(GGRAV*M/r);            // target orbit speed
		double vg = PI2*R/oapiGetPlanetPeriod(hRef)*cos(launch_lat);
		                                        // surface speed at launch position
		double vx0 = v0*sin(launch_azimuth);    // longitudinal velocity component
		double vx1 = vx0 + vg;                  // corrected for planet rotation
		double vy  = v0*cos(launch_azimuth);    // latitudinal velocity component
		B = atan2(vx1,vy);                      // effective launch azimuth
	}
	return PI05 - asin(sin(a)*sin(B));
}
Beispiel #2
0
// ==============================================================================================================================================
// Define planet excape orbit, Pos = vessel position, Esc = Escape vector, 
//
void Orbit::EscapeOrbit(OBJHANDLE ref,VECTOR3 Pos,VECTOR3 Esc,VECTOR3 normal)
{
	double myy=GC*oapiGetMass(ref);
	double rad=length(Pos);
	double Esc2=dotp(Esc,Esc);
	double ang=nangle(Pos,Esc,normal);
	double sma = -myy / Esc2;  // (constant)
	double sq=sin(ang);
	double q=rad*rad*sq*sq;
	double w=2.0*sma*rad*(cos(ang)-1.0);
	double ecc=sqrt((q-rad*sq*sqrt(q+2.0*w)+w)/(2.0*sma*sma)+1.0);
	double par=sma*(1.0-ecc*ecc);
 
	// Compute Tangential angle 
	double x=(par-rad)/(rad*ecc);
	if (x>1) x=1; if (x<-1) x=-1; 

	double tra=acos(x);
	double y=PI+acos(1.0/ecc);
	if (ang<y) tra=PI2-tra;

	double ta=tra2gamma(tra,ecc);

	VECTOR3 Vel=create_vector(normal,Pos,ta);
	Vel=set_length(Vel,sqrt(2*myy/rad - myy/sma));

	Elements(Pos,Vel,myy);
}
Beispiel #3
0
// ==============================================================================================================================================
//
bool Orbit::DefineOrbit(OBJHANDLE ref, VECTOR3 pos, VECTOR3 normal,double tra,double ecc)
{

	VECTOR3 ma  = create_vector(normal,pos,PI2-tra);
	VECTOR3 mi  = unit(crossp(normal,ma));

	double x,y;
	double myy = oapiGetMass(ref) * GC;
	double rad = length(pos);
	double par = rad * (1.0+ecc*cos(tra));
	double sma = par / (1.0-ecc*ecc);
	double vel = sqrt(2.0*myy/rad - myy/sma); // Ship's velocity	
	double smi = sqrt( fabs(sma) * par );
	double eca = tra2eca(tra,ecc);	

	INVALIDD(par*sma*vel) return false;

	if (ecc<1.0) { 
		x = -sma * sin(eca);
		y =  smi * cos(eca);
	} else {
		x = sma * sinh(eca);
		y = smi * cosh(eca);
	}
  
	double l = sqrt(x*x + y*y);

	VECTOR3 an = ma * (vel * x / l);
	VECTOR3 di = mi * (vel * y / l);
	
	Elements(pos,an+di,myy,false);

	return true;
}
Beispiel #4
0
// ==============================================================================================================================================
// Define orbit by using ship's position vector and Periapsis vector 
//
bool Orbit::ApproachOrbit(OBJHANDLE ref, VECTOR3 pos, VECTOR3 peri)
{
	VECTOR3 normal = crossp(pos,peri);
	VECTOR3 minv   = crossp(normal,peri);

	double x,y;
	double tra = nangle(pos,peri,normal);
	double myy = oapiGetMass(ref) * GC;
	double ped = length(peri);
	double rad = length(pos);
	double ecc = ( ped - rad ) / ( rad*cos(tra)-ped );  // Eccentricity of orbit	
	double sma = ped/(1.0-ecc); // Semi-major axis
	double par = sma * (1.0-ecc*ecc); // Parameter
	double vel = sqrt(2.0*myy/rad - myy/sma); // Ship's velocity
	double smi = sqrt( fabs(sma) * par );
	double eca = tra2eca(tra,ecc);	

	if (ecc<1) { 
		x = -sma * sin(eca);
		y =  smi * cos(eca);
	} else {
		x = sma * sinh(eca);
		y = smi * cosh(eca);
	}
  
	double l = sqrt(x*x + y*y);

	VECTOR3 an = unit(peri) * (vel * x / l);
	VECTOR3 di = unit(minv) * (vel * y / l);
	
	Elements(pos,an+di,myy,false);

	return true;
}
Beispiel #5
0
// ==============================================================================================================================================
//
void Orbit::Elements(OBJHANDLE body,OBJHANDLE ref) 
{
	VECTOR3 r,v;

  if (body==NULL || ref==NULL) return;

	double myy = oapiGetMass(ref) * GC;

	oapiGetRelativePos(body,ref,&r);
	oapiGetRelativeVel(body,ref,&v);

	Elements(r,v,myy);
}
Beispiel #6
0
// ==============================================================================================================================================
//
void Orbit::Elements(char *bo,char *re) 
{
	VECTOR3 r,v;

	OBJHANDLE body=oapiGetObjectByName(bo);
	OBJHANDLE ref=oapiGetObjectByName(re);

	if (body==NULL || ref==NULL) return;

	double myy = oapiGetMass(ref) * GC;

	oapiGetRelativePos(body,ref,&r);
	oapiGetRelativeVel(body,ref,&v);

	Elements(r,v,myy);
}
Beispiel #7
0
// ==============================================================================================================================================
//
void Orbit::GEO(OBJHANDLE ref)
{	
	if (!ref) {	GeoRef=NULL; return; }

	double obli  = oapiGetPlanetObliquity(ref);
	double peri  = oapiGetPlanetPeriod(ref);
	double trans = oapiGetPlanetTheta(ref);

	VECTOR3 rota, refd;
	PlanetAxis(obli,trans,&rota,&refd);

	VECTOR3 velo = crossp(rota,refd);
	double  myy  = oapiGetMass(ref)*GC;
	double  rad  = pow(fabs(peri)*sqrt(myy)/PI2, 2.0 / 3.0);
	double  vel  = sqrt(myy/rad);

	refd=set_length(refd,rad);
	velo=set_length(velo,vel);
	if (peri<0) velo=_V(0,0,0)-velo;

	Elements(refd,velo,myy);
	GeoRef=ref;
}
Beispiel #8
0
bool InstrVS::Redraw2D (SURFHANDLE surf)
{
	VECTOR3 V;
	double vspd, avspd;
	if (vessel->GetAirspeedVector (FRAME_HORIZON, V)) {
		vspd = V.y*0.1; // unit is 10m
		avspd = fabs(vspd);
	} else {
		vspd = avspd = 0.0;
	}

	static double texw = PANELEL_TEXW, texh = PANELEL_TEXH;
	static double scalecnt = texh-422.0+152.0;
	static int scaleunit = 15;
	static double viewh = 50.0;
	double ycnt, y0, y1, dy, vvspd, ddy;
	char *c, cbuf[12];
	bool centered = (fabs(vspd) <= 4.0);

	dy = vspd-floor(vspd);
	if (centered) {
		ycnt = scalecnt - vspd*scaleunit;
	} else {
		if (vspd > 0.0) ycnt = scalecnt - (5.0+dy)*scaleunit;
		else            ycnt = scalecnt + (5.0-dy)*scaleunit;
	}
	y0 = ycnt-viewh;
	y1 = ycnt+viewh;
	grp->Vtx[0+vtxofs].tv = grp->Vtx[1+vtxofs].tv = (float)(y0/texh);
	grp->Vtx[2+vtxofs].tv = grp->Vtx[3+vtxofs].tv = (float)(y1/texh);

	// copy labels onto scale
	const int labelx = 2;
	int i, j, n, vmin, vmax, iy, len, ysrc;
	int v0 = (int)floor(vspd);
	vmin = v0-3;
	if (vmin != pvmin) {
		pvmin = vmin;
		vmax = vmin+7;
		for (i = vmin; i <= vmax; i++) {
			sprintf (cbuf, "%03d", abs((i%100)*10));
			len = 3; // strlen(cbuf);
			if (centered) {
				if (!i) continue;
				iy = (int)scalecnt-i*scaleunit-5;
			} else {
				if (i > 0) iy = (int)scalecnt-(2+i-vmin)*scaleunit-5;
				else       iy = (int)scalecnt+(8-i+vmin)*scaleunit-5;
			}
			for (j = 0, c = cbuf; j < 3; c++, j++) {
				n = *c-'0';
				ysrc = (int)texh-265+n*8;
				if (i < 0) ysrc += 88;
				oapiBlt (surf, surf, labelx+j*6, iy, label_srcx, ysrc, 6, 8);
			}
		}
	}

	// max VS indicator
	OBJHANDLE hRef = vessel->GetSurfaceRef();
	if (hRef) {
		const double EPS = 1e-10;
		const double G = 6.67259e-11;
		double mu = G*oapiGetMass(hRef);
		ELEMENTS el;
		vessel->GetElements (hRef, el, 0);
		if (el.e == 1.0) el.e += EPS; // hack; what is the maximum radial velocity of a parabolic orbit?
		double vr_max = (el.e == 1.0 ? 0.0 : el.e * sqrt (mu/(el.a*(1.0-el.e*el.e))));

		vr_max *= 0.1;
		float yofs;
		if (vr_max < vspd+4 && vr_max > vspd-4)
			yofs = (float)((vr_max-vspd)*scaleunit*(48.0/50.0));
		else yofs = -60;
		static const float y0[3] = {vtape_ycnt, vtape_ycnt, vtape_ycnt-11};
		for (i = 0; i < 3; i++)
			grp->Vtx[vtxofs+i+12].y = y0[i]-yofs;

		if (-vr_max < vspd+4 && -vr_max > vspd-4)
			yofs = (float)((-vr_max-vspd)*scaleunit*(48.0/50.0));
		else yofs = -60;
		static const float y1[3] = {vtape_ycnt, vtape_ycnt, vtape_ycnt+11};
		for (i = 0; i < 3; i++)
			grp->Vtx[vtxofs+i+15].y = y1[i]-yofs;
	}

	// km/s indicator wheels
	sprintf (cbuf, "%05d", (((int)avspd)%10000)*10);
	for (i = 0; i < 2; i++) {
		float yofs = (float)(texh-111 - (cbuf[i]-'0')*15);
		if (avspd > 50.0) {
			const double scl[2] = {1e3,1e2};
			vvspd = avspd/scl[i];
			ddy = (vvspd-floor(vvspd))*scl[i];
			// number dials in rotation phase
			if (ddy < 0.5) yofs += (float)((0.5-ddy)*15.0);
			else if (ddy > scl[i]-0.5) yofs += (float)((scl[i]-0.5-ddy)*15.0);
		}
		for (j = 0; j < 4; j++) {
			grp->Vtx[4+i*4+vtxofs].tv = grp->Vtx[5+i*4+vtxofs].tv = yofs/(float)texh;
			grp->Vtx[6+i*4+vtxofs].tv = grp->Vtx[7+i*4+vtxofs].tv = (yofs+17.0f)/(float)texh;
		}
	}

	return false;
}
Beispiel #9
0
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);
}