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));
}
// ==============================================================================================================================================
// 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);
}
// ==============================================================================================================================================
//
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;
}
// ==============================================================================================================================================
// 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;
}
// ==============================================================================================================================================
//
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);
}
// ==============================================================================================================================================
//
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);
}
// ==============================================================================================================================================
//
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;
}
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;
}
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);
}
