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));
}
void AscentApMfd::UpdatePg_Prm (oapi::Sketchpad *skp)
{
char cbuf[256];
if (!ap->GetVessel()->status) {
sprintf (cbuf, "Launch azimuth: %0.1fº", ap->GetLaunchAzimuth()*DEG);
skp->Text (cw/2, (ch*3)/2, cbuf, strlen(cbuf));
sprintf (cbuf, "Orbit inc: %0.1fº", ap->GetTargetInclination()*DEG);
skp->Text (cw/2, (ch*5)/2, cbuf, strlen(cbuf));
sprintf (cbuf, "Orbit altitude: %0.1fkm", ap->GetOrbitAltitude()*1e-3);
skp->Text (cw/2, (ch*7)/2, cbuf, strlen(cbuf));
} else {
OBJHANDLE hRef = ap->GetVessel()->GetGravityRef();
double R = oapiGetSize(hRef);
double az_tgt = ap->GetTargetAzimuth();
double az_cur = ap->GetVessel()->GetYaw();
double az_err = fabs(az_cur-az_tgt);
if (az_err > PI) az_err = PI2-az_err;
if (az_cur < az_tgt) az_err = -az_err;
double pt_tgt = ap->GetTargetPitch();
double pt_cur = ap->GetVessel()->GetPitch();
double pt_err = pt_cur-pt_tgt;
double alt_tgt = ap->GetOrbitAltitude();
double alt_ap_cur, alt_pe_cur;
ap->GetVessel()->GetApDist(alt_ap_cur);
ap->GetVessel()->GetPeDist(alt_pe_cur);
alt_ap_cur -= R; alt_pe_cur -= R;
skp->Text (cw*13, ch*2, "Cur Tgt D", 15);
skp->Text (cw/2, ch*3, "Azimuth [º]", 11);
sprintf (cbuf, "%0.1lf", az_cur*DEG);
skp->Text (cw*13, ch*3, cbuf, strlen(cbuf));
sprintf (cbuf, "%0.1lf", az_tgt*DEG);
skp->Text (cw*20, ch*3, cbuf, strlen(cbuf));
sprintf (cbuf, "%+0.2f", az_err*DEG);
skp->Text (cw*27, ch*3, cbuf, strlen(cbuf));
skp->Text (cw/2, ch*4, "Pitch [º]", 9);
sprintf (cbuf, "%0.1lf", pt_cur*DEG);
skp->Text (cw*13, ch*4, cbuf, strlen(cbuf));
sprintf (cbuf, "%0.1lf", pt_tgt*DEG);
skp->Text (cw*20, ch*4, cbuf, strlen(cbuf));
sprintf (cbuf, "%+0.2lf", pt_err*DEG);
skp->Text (cw*27, ch*4, cbuf, strlen(cbuf));
skp->Text (cw/2, ch*5, "Ap.Alt [km]", 11);
sprintf (cbuf, "% 0.1lf", alt_ap_cur*1e-3);
skp->Text (cw*12, ch*5, cbuf, strlen(cbuf));
sprintf (cbuf, "%0.1lf", alt_tgt*1e-3);
skp->Text (cw*20, ch*5, cbuf, strlen(cbuf));
sprintf (cbuf, "%+0.2lf", (alt_ap_cur-alt_tgt)*1e-3);
skp->Text (cw*27, ch*5, cbuf, strlen(cbuf));
skp->Text (cw/2, ch*6, "Pe.Alt [km]", 11);
sprintf (cbuf, "% 0.1lf", alt_pe_cur*1e-3);
skp->Text (cw*12, ch*6, cbuf, strlen(cbuf));
sprintf (cbuf, "%0.1lf", alt_tgt*1e-3);
skp->Text (cw*20, ch*6, cbuf, strlen(cbuf));
sprintf (cbuf, "%+0.2lf", (alt_pe_cur-alt_tgt)*1e-3);
skp->Text (cw*27, ch*6, cbuf, strlen(cbuf));
}
}
void LEM::ToggleEVA()
{
ToggleEva = false;
if (CDREVA_IP) {
// Nothing for now, the EVA is ended, when the LEVA vessel calls StopEVA
/// \todo Support for 2 LEVA vessels
}
else {
VESSELSTATUS vs1;
GetStatus(vs1);
// The LM must be in landed state
if (vs1.status != 1) return;
CDREVA_IP = true;
OBJHANDLE hbody = GetGravityRef();
double radius = oapiGetSize(hbody);
vs1.vdata[0].x += 4.5 * sin(vs1.vdata[0].z) / radius;
vs1.vdata[0].y += 4.5 * cos(vs1.vdata[0].z) / radius;
char VName[256]="";
strcpy (VName, GetName()); strcat (VName, "-LEVA");
hLEVA = oapiCreateVessel(VName,"ProjectApollo/LEVA",vs1);
SwitchFocusToLeva = 10;
LEVA *leva = (LEVA *) oapiGetVesselInterface(hLEVA);
if (leva) {
LEVASettings evas;
evas.MissionNo = agc.GetApolloNo();
evas.Realism = Realism;
leva->SetEVAStats(evas);
}
}
}
// ==============================================================================================================================================
//
void Orbit::LEO(OBJHANDLE ref)
{
if (!ref) { LeoRef=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 = oapiGetSize(ref);
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);
LeoRef=ref;
}
bool InstrSpd::Redraw2D (SURFHANDLE surf)
{
double spd = vessel->GetGroundspeed()*0.1;
double vspd, aspd = fabs(spd);
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, ddy;
char *c, cbuf[12];
bool centered = (fabs(spd) <= 4.0);
dy = spd-floor(spd);
if (centered) {
ycnt = scalecnt - spd*scaleunit;
} else {
if (spd > 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 = 70;
int i, j, n, smin, smax, iy, len, ysrc;
int s0 = (int)floor(spd);
smin = s0-3;
if (smin != psmin) {
psmin = smin;
smax = smin+7;
for (i = smin; i <= smax; i++) {
sprintf (cbuf, "%03d", abs((i%100)*10));
len = 3;
if (centered) {
if (!i) continue;
iy = (int)scalecnt-i*scaleunit-5;
} else {
if (i > 0) iy = (int)scalecnt-(2+i-smin)*scaleunit-5;
else iy = (int)scalecnt+(8-i+smin)*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);
}
}
}
#ifdef UNDEF
// apoapsis/periapsis altitude indicators
OBJHANDLE hRef = vessel->GetSurfaceRef();
if (hRef) {
double rad = oapiGetSize (hRef);
double apalt, pealt;
vessel->GetApDist(apalt); apalt -= rad; apalt *= 1e-3;
vessel->GetPeDist(pealt); pealt -= rad; pealt *= 1e-3;
float yofs;
if (apalt < alt+4 && apalt > alt-4)
yofs = (float)((apalt-alt)*scaleunit*(48.0/50.0));
else yofs = -60;
static const float y0[3] = {atape_ycnt, atape_ycnt, atape_ycnt-11};
for (i = 0; i < 3; i++)
grp->Vtx[vtxofs+i+16].y = y0[i]-yofs;
if (pealt < alt+4 && pealt > alt-4)
yofs = (float)((pealt-alt)*scaleunit*(48.0/50.0));
else yofs = -60;
static const float y1[3] = {atape_ycnt, atape_ycnt, atape_ycnt+11};
for (i = 0; i < 3; i++)
grp->Vtx[vtxofs+i+19].y = y1[i]-yofs;
}
#endif
// km*1000 indicator wheels
sprintf (cbuf, "%06d", (((int)aspd)%100000)*10);
for (i = 0; i < 3; i++) {
float yofs = (float)(texh-111 - (cbuf[i]-'0')*15);
if (aspd > 50.0) {
const double scl[3] = {1e4,1e3,1e2};
vspd = aspd/scl[i];
ddy = (vspd-floor(vspd))*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;
}
int SoundEvent::play(SoundLib soundlib,
VESSEL *Vessel,
char *names ,
double *offset,
int *newbuffer,
double simcomputert,
double MissionTime,
int mode,
double timeremaining,
double timeafterpdi,
double timetoapproach,
int flags,
int volume)
{
int kk;
char buffers[255];
double altitude;
double vlon,vlat,vrad;
double timetopdi;
double timesincepdi;
double deltaoffset =0.;
if (!isValid())
return(false);
// is Sound still playing ?
// if yes let it play
// TRACESETUP("SOUNDEVENT PLAY");
if(lastplayed >= 0)
{
if(soundevents[lastplayed].offset == -1)
{
if (IsPlaying())
{
// TRACE("EN TRAIN DE JOUER");
return(false);
}
}
else if (IsPlaying())
{
if (soundevents[lastplayed+1].offset > soundevents[lastplayed].offset)
{
if(Finish((double) soundevents[lastplayed+1].offset - 0.1))
{
}
else
return(false);
}
}
}
// no more sounds to be played just return
if(soundevents[lastplayed+1].met == 0)
return(false);
// sprintf(buffers,"ENTREE PLAY MODE %d timeremaining %f timeafterpdi %f altitude %f",
// mode,timeremaining,timeafterpdi,altitude);
// TRACE(buffers);
timetopdi = timeremaining -(MissionTime - simcomputert);
timesincepdi = timeafterpdi +(MissionTime - simcomputert);
timetoapproach = timetoapproach -(MissionTime -simcomputert);
if (mode == 0)
{
/* sprintf(buffers,"AVANT PDI IGNIT %f %f %f ",
timetopdi, MissionTime, simcomputert);
sprintf(oapiDebugString(),"%s",buffers);
*/
}
if (mode >= 1 && mode < 3)
{
OBJHANDLE hbody=Vessel->GetGravityRef();
double bradius=oapiGetSize(hbody);
Vessel->GetEquPos(vlon, vlat, vrad);
double cgelev=Vessel->GetCOG_elev();
altitude=vrad-bradius-cgelev;
/* sprintf(buffers,"AFTER PDI : TIMEAFTER %f ALTITUDE %f",
timesincepdi,altitude);
sprintf(oapiDebugString(),"%s",buffers);
*/
}
kk = lastplayed+1;
sprintf(buffers,"%f",soundevents[kk].altitude);
// TRACE (buffers);
// sprintf(buffers,"%f",altitude);
// TRACE (buffers);
// if mode = 1 BRAKING it's altitude
// if mode = 0 MONITOR it's time to ignition
if (mode == 0)
{
// TRACE("TEST PDI IGNIT");
sprintf(buffers,"TEST PDI IGNIT NEXTEVENT %d %f %s VERSUS %f",kk,
soundevents[kk].timetoignition,soundevents[kk].filenames,
timetopdi);
// sprintf(oapiDebugString(),"%s",buffers);
// TRACE(buffers);
/* skip too old sounds */
while ( (soundevents[kk].timetoignition > timetopdi)
&& (soundevents[kk+1].timetoignition > timetopdi)
)
kk++;
if (soundevents[kk].timetoignition < timetopdi)
return 0;
deltaoffset = soundevents[kk].timetoignition - timetopdi;
}
else if (mode == 1)
{
//sprintf(buffers,"TIME AFTER PDI %f", timesincepdi);
//TRACE(buffers);
if ((timeafterpdi < 160))
{
// TRACE("TIME AFTER");
sprintf(buffers,"TIME AFTER %f %f",soundevents[kk].timeafterignition,
timesincepdi);
// TRACE(buffers);
sprintf(buffers,"TEST AFTER PDI NEXTEVENT %d %f %s VERSUS %f",kk,
soundevents[kk].timeafterignition,soundevents[kk].filenames,
timesincepdi);
// sprintf(oapiDebugString(),"%s",buffers);
while ( (soundevents[kk].timeafterignition < timesincepdi)
&& (soundevents[kk+1].timeafterignition < timesincepdi)
&& (!soundevents[kk].mandatory)
)
kk++;
if (soundevents[kk].timeafterignition>timesincepdi)
return 0;
}
else
{
// TRACE("TIME TO APPROACH");
sprintf(buffers,"TIME TO APP %f %f",soundevents[kk].timetoapproach,
timetoapproach);
// TRACE(buffers);
sprintf(buffers,"TEST TO APPROACH NEXTEVENT %d %f %s VERSUS %f",kk,
soundevents[kk].timetoapproach,soundevents[kk].filenames,
timetoapproach);
// sprintf(oapiDebugString(),"%s",buffers);
while ( (soundevents[kk].timetoapproach > timetoapproach)
&& (soundevents[kk+1].timetoapproach > timetoapproach)
&& (!soundevents[kk].mandatory)
)
kk++;
if (soundevents[kk].timetoapproach<timetoapproach)
return 0;
}
}
else if (mode == 2)
{
// TRACE("TEST ALTITUDE");
sprintf(buffers,"ALT %d %f %f",mode,soundevents[kk].altitude,altitude);
// TRACE(buffers);
// sprintf(buffers,"TEST ALTI NEXTEVENT %d %f %s VERSUS %f MODE %d",kk,
// soundevents[kk].altitude,soundevents[kk].filenames,
// altitude,mode);
// sprintf(oapiDebugString(),"%s",buffers);
while (soundevents[kk].mode != mode)
{
kk++;
}
while ( (soundevents[kk].altitude > altitude)
&& (soundevents[kk+1].altitude > altitude)
&& (!soundevents[kk].mandatory)
)
kk++;
if (soundevents[kk].altitude<altitude)
return 0;
}
else if (mode == 3)
{
if (soundevents[kk].met > MissionTime)
return 0;
deltaoffset = 0.0;
}
else return 0;
// TRACE("ON VA JOUER ");
// TRACE(soundevents[kk].filenames);
strcpy(names,soundevents[kk].filenames);
*offset = (double) soundevents[kk].offset + deltaoffset;
*newbuffer = true;
if(lastplayed >= 0)
{
if (!strcmp(soundevents[kk].filenames,soundevents[lastplayed].filenames))
*newbuffer = false;
}
lastplayed = kk;
// sprintf(buffers,"SON %d NEW %d OFFSET %f", kk, *newbuffer,*offset);
// TRACE(buffers);
return 1;
}
void AscentAP::Update (double simt)
{
const double eps=1e-5;
tgt.az = CalcTargetAzimuth();
tgt.pitch = CalcTargetPitch();
if (met_active)
met = simt-t_launch;
if (active) {
if (vessel->status == 0) {
if (met < 0.0) {
vessel->SetEngineLevel (ENGINE_MAIN, min (1.0, (SRB_STABILISATION_TIME+met)*0.4));
} else {
if (vessel->status == 0) {
t_launch = vessel->t0 = simt;
vessel->pET->IgniteSRBs ();
vessel->status = 1;
}
}
}
if (vessel->status < 3) {
if (met_meco < 0.0) {
double apalt;
OBJHANDLE hRef = vessel->GetApDist(apalt);
bool fuel_down = (vessel->pET ? vessel->pET->GetMainPropellantMass() < 10.0 : false);
apalt -= oapiGetSize (hRef);
if (apalt >= tgt_alt || fuel_down) {
vessel->SetThrusterGroupLevel (THGROUP_MAIN, 0.0); // MECO
met_meco = met;
} else {
vessel->SetThrusterGroupLevel (THGROUP_MAIN, 1.0);
// make sure the user can't manually throttle down the SSME
}
} else if (met-met_meco >= 10.0) {
vessel->SeparateTank();
double apalt;
OBJHANDLE hRef = vessel->GetApDist(apalt);
apalt -= oapiGetSize (hRef);
if (apalt + 1e3 < tgt_alt) {
schedule_oms1 = met+20.0;
}
}
} else if (schedule_oms1 > 0.0) {
if (met >= schedule_oms1) {
vessel->SetThrusterGroupLevel (THGROUP_MAIN, 1.0);
schedule_oms1 = -1.0;
met_oms1_start = met;
}
} else if (met_oms1_start > 0.0) {
double apalt;
OBJHANDLE hRef = vessel->GetApDist(apalt);
apalt -= oapiGetSize (hRef);
if (apalt >= tgt_alt) {
vessel->SetThrusterGroupLevel (THGROUP_MAIN, 0.0); // OMS1 end
met_oms1_start = -1.0;
}
} else {
if (met_oms_start < 0.0) {
OBJHANDLE hRef = vessel->GetSurfaceRef();
ELEMENTS el;
ORBITPARAM prm;
vessel->GetElements(hRef, el, &prm);
if (prm.ApT < 70.0) {
vessel->SetThrusterGroupLevel (THGROUP_MAIN, 1.0); // OMS ignition
met_oms_start = met;
}
} else if (met_oms_end < 0.0) {
double perad, aprad, pealt, ecc;
OBJHANDLE hRef = vessel->GetPeDist(perad);
hRef = vessel->GetApDist(aprad);
pealt = perad-oapiGetSize(hRef);
ecc = (aprad-perad)/(aprad+perad);
if (ecc < ecc_min) ecc_min = ecc;
if (ecc > ecc_min+eps) {
vessel->SetThrusterGroupLevel (THGROUP_MAIN, 0.0); // OMS cut off
met_oms_end = met;
active = false; // turn off ascent autopilot
}
}
}
}
}
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);
}
// ==============================================================================================================================================
//
void SyncMFD::Update(HDC hDC)
{
int pos;
int ld=MFD->LineHeight;
int fbp=MFD->FirstButton;
char *Mnu = {"BaseSync v3.2\0"};
int width=MFD->width;
int height=MFD->height;
double w=(double) width;
double h=(double) height;
int i;
VECTOR3 Rot,Off,opos;
double obli,trans,per,offset,diff,p,op,t,time,lon,lat;
double trl,heading;
char name[32];
static const int MAXSOLN = 160;
double times[MAXSOLN],diffs[MAXSOLN],heads[MAXSOLN];
int sol_found = 0;
double max_diff=1000;
bool draw_text=false;
Orbit LEO,ShipOrbit,Ecliptic;
SetTextColor(hDC,white);
SetTextAlign(hDC,TA_LEFT);
TextOut(hDC,5,1,Mnu, (int)strlen(Mnu));
if (mode.enc==0) Text(hDC,5,1+ld,"Latitude");
if (mode.enc==1) Text(hDC,5,1+ld,"Closest passage");
if (mode.enc==2) Text(hDC,5,1+ld,"Apoapsis");
if (mode.enc==3) Text(hDC,5,1+ld,"Periapsis");
if (sync_num<1) sync_num=1;
if (sync_num>99) sync_num=99;
OBJHANDLE ref = oapiGetObjectByName(trgt->ref);
if (ref==NULL) {
usingGS2 = false;
trgt = &bstrgt;
ref=ship->GetSurfaceRef();
}
if (ref==NULL) return;
ShipOrbit.Elements(ship->GetHandle(), ref);
Ecliptic.Ecliptic();
oapiGetObjectName(ref, trgt->ref, 31);
// Rotation elements
obli = oapiGetPlanetObliquity(ref);
trans = oapiGetPlanetTheta(ref);
per = oapiGetPlanetPeriod(ref);
offset = oapiGetPlanetCurrentRotation(ref);
// LEO
LEO.LEO(ref);
if (!usingGS2) {
OBJHANDLE tgt = oapiGetBaseByName(ref, trgt->name);
if (tgt) {
oapiGetObjectName(tgt,trgt->name,32);
oapiGetBaseEquPos(tgt,&trgt->lon, &trgt->lat);
}
else strcpy(trgt->name,"Surface");
}
double trle=ShipOrbit.TrlOfNode(&LEO);
double EqI=angle(ShipOrbit.norv, LEO.norv);
double ang=asin(sin(trgt->lat) / sin(EqI));
double apos=limit(trle+ang);
double bpos=limit(trle+PI-ang);
double atime=ShipOrbit.TimeTo(apos);
double btime=ShipOrbit.TimeTo(bpos);
double dist=ShipOrbit.PeriapsisDistance();
double apodist = dist=ShipOrbit.AopapsisDistance();
if (dist<oapiGetSize(ref)) dist=oapiGetSize(ref);
double zoom=w/(2.55 * (apodist + apodist + dist) / 3.0);
double r=oapiGetSize(ref)*zoom;
double x=w/2,y=h/2;
double intpos=0;
if (display_texts&2) {
SelectObject(hDC,solid_pen_dgrey);
DrawEllipse(hDC,x-r,y-r,x+r,y+r,w,h);
if (mode.enc==0) { // Latitude
if ((sync_sel&1)==0) SelectObject(hDC,solid_pen_y), intpos=apos;
else SelectObject(hDC,solid_pen_dgrey);
r=ShipOrbit.Radius(apos)*zoom;
DrawLine(hDC,x,y,x+r*cos(apos),x-r*sin(apos),w,h,false);
if ((sync_sel&1)==1) SelectObject(hDC,solid_pen_y), intpos=bpos;
else SelectObject(hDC,solid_pen_dgrey);
r=ShipOrbit.Radius(bpos)*zoom;
DrawLine(hDC,x,y,x+r*cos(bpos),x-r*sin(bpos),w,h,false);
}
if (mode.enc==1) { // Closest Passage
r=ShipOrbit.Radius(sync_trl)*zoom;
intpos=sync_trl;
SelectObject(hDC,solid_pen_y);
DrawLine(hDC,x,y,x+r*cos(sync_trl),x-r*sin(sync_trl),w,h,false);
}
if (mode.enc==2 || mode.enc==3) { // Aopapsis Periapsis
SelectObject(hDC,solid_pen_y);
r=ShipOrbit.Radius(sync_trl)*zoom;
DrawLine(hDC,x,y,x+r*cos(sync_trl),x-r*sin(sync_trl),w,h,false);
SelectObject(hDC,solid_pen_grey);
r=ShipOrbit.Radius(sync_line)*zoom;
DrawLine(hDC,x,y,x+r*cos(sync_line),x-r*sin(sync_line),w,h,false);
}
ShipOrbit.SetProjection(&ShipOrbit);
ShipOrbit.GDIDraw(hDC,green,w,h,zoom,true,true);
if (mode.deo) {
r=ShipOrbit.Radius(deo.trlBurn)*zoom;
SelectObject(hDC,solid_pen_white);
DrawLine(hDC,x,y,x+r*cos(deo.trlBurn),x-r*sin(deo.trlBurn),w,h,false);
}
}
if (EqI>=trgt->lat || mode.enc!=0) {
// Usual case... target in range
draw_text=true;
SetTextColor(hDC,green);
pos=(fbp+(ld*8));
if (display_texts&1 && !mode.deo) {
if (mode.enc==0) {
Text(hDC,5,pos," #: Time:");
Text(hDC,width/2,pos,"Lon Diff:"), pos+=ld;
} else {
Text(hDC,5,pos," #: Time:");
Text(hDC,5+width/3,pos," Dist:");
Text(hDC,5+width*2/3,pos," Heading:"), pos+=ld;
}
}
if (atime<btime && atime>0) sync_sel=0;
else if (btime>0) sync_sel=1;
else sync_sel=0;
for (i=0;i<MAXSOLN;i++) {
times[i]=0;
diffs[i]=0;
}
sync_min = -1;
if (ShipOrbit.ecc<1 && mode.enc==0) {
for (i=0;i<MAXSOLN;i++) {
if (i&1) time=MAX(atime, btime);
else time=MIN(atime, btime);
if (time==atime) op=apos;
else op=bpos;
p=(double)i;
if (i>1) time+=ShipOrbit.Period()*floor(p/2);
t = time/86400;
PlanetAxis(obli,trans,offset,per,t,&Rot,&Off);
opos=ShipOrbit.Position(op);
LonLat(opos,Rot,Off,&lon,&lat);
diff=lon-trgt->lon;
if (fabs(diff)<max_diff) {
max_diff=fabs(diff), sync_time=time, sync_trl=op;
sync_min=i;
}
times[i]=time;
diffs[i]=diff;
if (time > 0.0 && diff >0.0) {
sol_found++;
if (sol_found==sync_num) break;
}
}
}
max_diff=1e10;
if (ShipOrbit.ecc<1 && mode.enc==1) {
double posit=ShipOrbit.trl;
for (i=0;i<MAXSOLN;i++) {
InterpolateClosestPassage(ref,&ShipOrbit,trgt->lon,trgt->lat,posit,&diff,&time,&heading,&trl);
posit=trl+PI05;
if (diff<max_diff && diff>0) {
max_diff=diff, sync_time=time, sync_trl=limit(trl);
sync_min=i;
}
heads[i]=heading;
times[i]=time;
diffs[i]=diff;
if (time > 0.0 && diff >0.0) {
sol_found++;
if (sol_found==sync_num) break;
}
}
}
if (ShipOrbit.ecc<1 && (mode.enc==2 || mode.enc==3)) {
for (i=0;i<MAXSOLN;i++) {
if (mode.enc==2) sync_line=limit(ShipOrbit.lpe+PI);
else sync_line=limit(ShipOrbit.lpe);
time=ShipOrbit.TimeTo(sync_line) + ShipOrbit.Period() * (double)i;
t = time / 86400.0;
PlanetAxis(obli,trans,offset,per,t,&Rot,&Off);
VECTOR3 gpv=VectorByLonLat(Rot,Off,trgt->lon,trgt->lat);
VECTOR3 pos=ShipOrbit.Position(sync_line);
diff = angle(gpv,pos);
heading = nangle(pos-gpv,Rot,gpv);
ShipOrbit.Longitude(gpv,NULL,NULL,&trl);
if (diff<max_diff && diff>0) {
max_diff=diff, sync_time=time, sync_trl=limit(trl);
sync_min=i;
}
if (time==0) time=0.1;
heads[i]=heading;
times[i]=time;
diffs[i]=diff;
if (time > 0.0 && diff >0.0) {
sol_found++;
if (sol_found==sync_num) break;
}
}
}
// Hyperbolic Orbit
if (ShipOrbit.ecc>=1 && (atime>0 || btime>0)) {
mode.enc=0;
for (i=0;i<2;i++) {
if (atime>0 && btime>0) {
if (i&1) time=MAX(atime, btime);
else time=MIN(atime, btime);
}
else time=(atime>0 ? atime : btime);
op = (time==atime? apos : bpos);
p=(double)i;
if (i>1) time+=ShipOrbit.Period()*floor(p/2);
t = time/86400;
PlanetAxis(obli,trans,offset,per,t,&Rot,&Off);
opos=ShipOrbit.Position(op);
LonLat(opos,Rot,Off,&lon,&lat);
diff=lon-trgt->lon;
if (fabs(diff)<max_diff) {
max_diff=fabs(diff), sync_time=time, sync_trl=op;
sync_min=i;
}
times[i]=time;
diffs[i]=diff;
if (atime<0 || btime<0) break;
}
}
if (sync_min > -1) {
sol.num = sync_min+1;
sol.tSol = times[sync_min];
sol.dist = diffs[sync_min];
sol.hdg = heads[sync_min];
sol.dataValid = true;
} else {
sol.dataValid = false;
}
double rad=oapiGetSize(ref);
int no=0;
int disp_i = 1;
if (display_texts&1 && !mode.deo) {
for (i=0;i<MAXSOLN;i++) {
if (i==sync_min) SetTextColor(hDC,lyellow);
else SetTextColor(hDC,lgreen);
if (times[i]>0.0 && diffs[i]>=0.0) {
if (disp_i >= sync_dispmin && disp_i <= sync_dispmin+7) {
if (mode.enc==0) {
sprintf(name,"%2d: ",disp_i);
Text(hDC,5,pos,name,times[i]);
TextA(hDC,width/2,pos,"",diffs[i]*DEG), pos+=ld;
} else { // enc_mode 1,2,3
no++;
sprintf(name,"%2d: ",disp_i);
Text(hDC,5,pos,name,times[i]);
Text(hDC,5+width/3,pos," ",diffs[i]*rad);
TextA(hDC,5+width*2/3,pos," ",heads[i]*DEG), pos+=ld;
}
}
disp_i++;
if (disp_i > sync_num) break;
}
}
}
sync_sel+=sync_min;
} else {
// We are in Latitude mode, and the target is outside of our inclination - i.e. no solution
sol.dataValid = false;
SetTextAlign(hDC,TA_CENTER);
SetTextColor(hDC,yellow);
pos=(fbp+(ld*8));
Text(hDC,width/2,pos,"Target Out of Range"); pos+=ld;
}
if (mode.dir) {
// Calculate burn for plane change correction in DIRECT mode
if (EqI<trgt->lat && mode.enc==0) {
double trl;
PlanetAxis(obli,trans,offset,per,0,&Rot,&Off);
VECTOR3 gpv = VectorByLonLat(Rot,Off,trgt->lon,trgt->lat);
ShipOrbit.Longitude(gpv,NULL,NULL,&trl);
sync_time=ShipOrbit.TimeTo(trl);
sync_trl=trl;
}
time_to_int=sync_time;
double trl=sync_trl;
PlanetAxis(obli,trans,offset,per,time_to_int/86400.0,&Rot,&Off);
VECTOR3 pos = ShipOrbit.Position(trl);
VECTOR3 gpv = VectorByLonLat(Rot,Off,trgt->lon,trgt->lat);
VECTOR3 lan = crossp(gpv, pos);
VECTOR3 nor = ShipOrbit.norv;
ShipOrbit.Longitude(lan,NULL,NULL,&sol.trlBurn);
sol.rIn = fabs(asin(dotp(gpv,nor)));
double a = ShipOrbit.TimeToPoint(sol.trlBurn);
double b = ShipOrbit.TimeToPoint(limit(sol.trlBurn+PI));
if (fabs(a)<fabs(b)) sol.tToBurn =a, sol.nmlBurn=true;
else sol.tToBurn =b, sol.nmlBurn=false;
sol.dV = sol.rIn*ShipOrbit.ang/ShipOrbit.Radius(sol.trlBurn);
sol.tBurn =BurnTimeBydV(sol.dV,ship);
} else {
// Calculate burn for plane change correction in EQUATORIAL mode
sol.trlBurn=ShipOrbit.TrlOfNode(&LEO);
sol.rIn=MAX(trgt->lat-EqI,0);
double a=ShipOrbit.TimeToPoint(sol.trlBurn);
double b=ShipOrbit.TimeToPoint(limit(sol.trlBurn+PI));
if (fabs(a)<fabs(b)) sol.tToBurn =a, sol.nmlBurn=true;
else sol.tToBurn =b, sol.nmlBurn=false;
sol.dV = sol.rIn*ShipOrbit.ang/ShipOrbit.Radius(sol.trlBurn);
sol.tBurn =BurnTimeBydV(sol.dV,ship);
}
if (display_texts&2) {
ShipOrbit.DrawPlaneIntersection(hDC,sol.trlBurn,w/2,h/2,zoom,grey);
}
SetTextAlign(hDC,TA_LEFT);
pos=1;
SetTextColor(hDC,grey);
if (usingGS2) {
Text(hDC,width*1/2,pos,"Linked ","to GS"); pos+=ld;
// Check if GS2 changed target
if (bstrgt.lat != gs2trgt->lat || bstrgt.lon != gs2trgt->lon) {
strcpy(bstrgt.ref, gs2trgt->ref);
strcpy(bstrgt.name, gs2trgt->name);
bstrgt.lat = gs2trgt->lat;
bstrgt.lon = gs2trgt->lon;
bstrgt.alt = gs2trgt->alt;
bstrgt.ang = gs2trgt->ang;
bstrgt.ant = gs2trgt->ant;
}
} else {
Text(hDC,width*1/2,pos,"Ref ",trgt->ref); pos+=ld;
}
Text(hDC,width*1/2,pos,"Tgt ",trgt->name); pos+=ld;
if (display_texts&1) {
if (!mode.deo) {
// Display the burn solution for plane change
deo.dataValid = false;
pos=fbp;
SetTextColor(hDC,lgreen);
if (trgt->lat<0) TextA(hDC,5,pos,"Lat ",fabs(DEG*trgt->lat),"S"), pos+=ld;
else TextA(hDC,5,pos,"Lat ",DEG*trgt->lat,"N"), pos+=ld;
if (trgt->lon<0) TextA(hDC,5,pos,"Lon ",fabs(DEG*trgt->lon),"W"), pos+=ld;
else TextA(hDC,5,pos,"Lon ",DEG*trgt->lon,"E"), pos+=ld;
TextA(hDC,5,pos,"EqI ",EqI*DEG), pos+=ld;
PlanetAxis(ref,0,&Rot,&Off);
VECTOR3 base_pos = VectorByLonLat(Rot,Off,trgt->lon,trgt->lat);
VECTOR3 ship_pos; ship->GetRelativePos(ref,ship_pos);
VECTOR3 ship_vel; ship->GetRelativeVel(ref,ship_vel);
VECTOR3 gsp=GroundSpeedVector(ref,ship);
VECTOR3 zero=crossp(ship_pos,ship_vel);
double head = nangle(gsp,zero,ship_pos);
double baseHeight = oapiGetSize(ref);
#if ORBITER_VERSION == 2016
ELEVHANDLE eh = oapiElevationManager(ref);
if (eh) {
baseHeight += oapiSurfaceElevation(ref, trgt->lon, trgt->lat);
}
#endif
TextA(hDC,5,pos,"Hed ",head*DEG), pos+=ld;
Text(hDC,5,pos, "GSp ",length(gsp)), pos+=ld;
Text(hDC,5,pos, "Dst ",angle(ship_pos,base_pos)*baseHeight), pos+=ld;
if (sol.dataValid) {
double altitude = length(ShipOrbit.Position(intpos));
altitude -= baseHeight;
Text(hDC,5,pos, "Alt ",altitude);
}
pos+=ld;
pos=fbp;
int xx=width/2;
SetTextColor(hDC,green);
if (mode.dir) Text(hDC,xx,pos,"Direct:"), pos+=ld;
else Text(hDC,xx,pos,"Equator:"), pos+=ld;
SetTextColor(hDC,lgreen);
TextA(hDC,xx,pos,"RIn ",sol.rIn*DEG), pos+=ld;
TextA(hDC,xx,pos,"LAN ",sol.trlBurn*DEG), pos+=ld;
Text(hDC,xx,pos," Tn ",sol.tToBurn), pos+=ld;
if (abs(sol.tBurn) > 0.1) {
if (sol.nmlBurn) Text(hDC,xx,pos,"PlC ",sol.tBurn,"s (+)"), pos+=ld;
else Text(hDC,xx,pos,"PlC ",sol.tBurn,"s (-)"), pos+=ld;
Text(hDC,xx,pos," dV ",sol.dV,"m/s"), pos+=ld;
} else {
Text(hDC,xx,pos,"PlC 0.000s"), pos+=ld;
Text(hDC,xx,pos," dV 0.000m/s"), pos+=ld;
}
if (sol.dataValid) {
burn.dV = sol.dV;
burn.orientation = sol.nmlBurn ? 1 : -1;
burn.tToInstBurn = sol.tToBurn;
burn.dataValid = true;
} else {
burn.dataValid = false;
}
} else {
// Display the deorbit parameters and burn solution
pos=fbp;
SetTextColor(hDC,white);
Text(hDC,5,pos,"De-Orbit Program"), pos+=2*ld;
SetTextColor(hDC,lgreen);
if (usingGS2) {
Text(hDC,5,pos, "Ang, Ant, Alt from GS"), pos+=ld;
TextA(hDC,5,pos,"GS Ang ",trgt->ang*DEG), pos+=ld;
TextA(hDC,5,pos,"GS Ant ",trgt->ant*DEG), pos+=ld;
Text(hDC,5,pos, "GS Alt ",trgt->alt,"km"), pos+=2*ld;
} else {
TextA(hDC,5,pos,"Ang ",bstrgt.ang*DEG), pos+=ld;
TextA(hDC,5,pos,"Ant ",bstrgt.ant*DEG), pos+=ld;
Text(hDC,5,pos, "Alt ",bstrgt.alt,"km"), pos+=2*ld;
}
double pre;
ComputeDeOrbit(ShipOrbit.myy,ShipOrbit.rad,trgt->alt*1000+oapiGetSize(ref),trgt->ang,&pre,&deo.dV);
deo.dV=ShipOrbit.vel-deo.dV;
deo.trlBurn = limit(sync_trl - trgt->ant - pre);
deo.tInstBurn = ShipOrbit.TimeToPoint(deo.trlBurn);
deo.tBurn = BurnTimeBydV(deo.dV,ship);
deo.tToBurn = deo.tInstBurn - 0.5 * deo.tBurn;
Text(hDC,5,pos, "TBn ",deo.tToBurn,"s"), pos+=ld;
TextA(hDC,5,pos,"TrL ",deo.trlBurn*DEG), pos+=ld;
Text(hDC,5,pos, " dV ",deo.dV,"m/s"), pos+=ld;
Text(hDC,5,pos, " BT ",deo.tBurn,"s"), pos+=2*ld;
if (ShipOrbit.ecc>0.015) {
SetTextColor(hDC,lgreen);
Text(hDC,5,pos,"De-orbit burn data only accurate"); pos+=ld;
Text(hDC,5,pos,"if your Ecc is 0.015 or less."); pos+=ld;
Text(hDC,5,pos,"(If burn complete, please ignore.)"); pos+=ld;pos+=ld;
}
if (!mode.dir) {
SetTextColor(hDC,lgreen);
Text(hDC,5,pos,"Use \"Direct\" mode to re-synchronize"); pos+=ld;
Text(hDC,5,pos,"the approach after the de-orbit");
}
deo.dataValid = true;
burn.dV = -deo.dV;
burn.orientation = 0;
burn.tToInstBurn = deo.tToBurn;
burn.dataValid = true;
}
}
if (!MMPut_done) {
mma.PutMMStruct("BaseSyncTarget", &bstrgt);
mma.PutMMStruct("BaseSyncMode", &mode);
mma.PutMMStruct("BaseSyncSolution", &sol);
mma.PutMMStruct("BaseSyncDeorbit", &deo);
mma.PutMMStruct("BaseSyncBurn", &burn); // To be removed after BTC synchronization
if (burn.dataValid)
{
mma.Put("dv", burn.dV);
mma.Put("InstantaneousBurnTime", burn.tToInstBurn);
mma.Put("Orientation", burn.orientation);
}
else
{
mma.Delete("dv");
mma.Delete("InstantaneousBurnTime");
mma.Delete("Orientation");
}
MMPut_done = true;
}
}
// ==============================================================================================================================================
//
bool SyncMFD::InterpolateClosestPassage(OBJHANDLE ref, Orbit *src, double lon,double lat,double orbit,double *diff,double *tim,double *head,double *tr)
{
VECTOR3 Rot,Off;
Orbit LEO;
int i;
double obli = 0.0;
double trans = 0.0;
double per = 0.0;
double offset = 0.0;
double size = 0.0;
double time = 0.0;
// Rotation elements
obli = oapiGetPlanetObliquity(ref);
trans = oapiGetPlanetTheta(ref);
per = oapiGetPlanetPeriod(ref);
offset = oapiGetPlanetCurrentRotation(ref);
size = oapiGetSize(ref);
LEO.LEO(ref);
double step = PI2/16.01;
double start = orbit;
double end = orbit+PI2+(step/2.0);
double trl, dst2;
double dot,old_dot=0;
VECTOR3 pos = _V(0,0,0);
VECTOR3 vel = _V(0,0,0);
VECTOR3 gpv = _V(0,0,0);
VECTOR3 spd = _V(0,0,0);
VECTOR3 relv = _V(0,0,0);
VECTOR3 relp = _V(0,0,0);
bool no_intercept=true;
bool first=true;
for (trl=start;trl<end;trl+=step) {
// Caluclate time to a location
time=Trl2Time(trl,src)/86400.0;
// Caluclate Planets Rotation offset at that time
PlanetAxis(obli,trans,offset,per,time,&Rot,&Off);
// Calculate Ship's position vector and distance^2
pos = src->Position(trl);
dst2 = dotp(pos,pos);
// Reset the distance to correspond a surface position.
pos = set_length(pos,size);
// Compute ship's surface velocity vector direction
vel = crossp(src->norv,pos);
// Compute ship's surface velocity vector magnitude
vel = set_length(vel,src->ang*size/dst2);
// Compute base's position vector
gpv = VectorByLonLat(Rot,Off,lon,lat)*size;
// Compute speed of the base
spd = GroundSpeedVector(ref,time,lon,lat);
// Copmute relative position to the base
relv = (vel-spd);
relp = (pos-gpv);
dot = dotp(relv,relp);
if (old_dot<0 && dot>0 && !first) {
if (angle(pos,gpv)<PI05) {
start=trl-step;
no_intercept=false;
break;
}
}
first=false;
old_dot=dot;
}
// Return N/A if no passage points found
if (no_intercept) {
if (head) *head = 0;
if (diff) *diff = 0;
if (tim) *tim = 0;
if (tr) *tr = start+PI2-PI05;
return false;
}
trl=start;
old_dot=-1; // We are approaching the base
double trll=0;
no_intercept=true;
for (i=0;i<24;i++) {
step/=2.0;
// Caluclate time to a location
time=Trl2Time(trl,src)/86400.0;
// Caluclate Planets Rotation offset at that time
PlanetAxis(obli,trans,offset,per,time,&Rot,&Off);
// Calculate Ship's position vector and distance^2
pos = src->Position(trl);
dst2 = dotp(pos,pos);
// Reset the distance to correspond a surface position.
pos = set_length(pos,size);
// Compute ship's surface velocity vector direction
vel = crossp(src->norv,pos);
// Compute ship's surface velocity vector magnitude
vel = set_length(vel,src->ang*size/dst2);
// Compute base's position vector
gpv = VectorByLonLat(Rot,Off,lon,lat)*size;
// Compute speed of the base
spd = GroundSpeedVector(ref,time,lon,lat);
// Copmute relative position to the base
relv = (vel-spd);
relp = (pos-gpv);
dot = dotp(relv,relp);
trll=trl;
if (dot==0) {
no_intercept=false;
break; // closest position archived, break
}
if (dot>0) { // base is behind of us, step back
trl=trl-step;
no_intercept=false; // Verify, We have a base passage.
}
else trl=trl+step; // base is front of us, cuntinue stepping
}
double dif=angle(pos, gpv);
VECTOR3 zero=crossp(pos,spd);
if (head) *head = nangle(relp,zero,pos);
if (diff) *diff = dif;
if (tim) *tim = time*86400.0;
if (tr) *tr = trll;
return true;
}
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
}
}
bool InstrHSI::Redraw2D (SURFHANDLE surf)
{
const float horzx = (float)(texw-312), horzy = (float)(texh-252);
DWORD tp;
int i, j, vofs;
double c, sinc, cosc, brg, slope;
double yaw = vessel->GetYaw(); if (yaw < 0.0) yaw += PI2;
double siny = sin(yaw), cosy = cos(yaw);
dev = 0.0;
NAVHANDLE nv = vessel->GetNavSource (0);
if (nv) {
tp = oapiGetNavType(nv);
if (tp != TRANSMITTER_VOR && tp != TRANSMITTER_ILS)
nv = NULL;
}
if (nv != nav) {
if (nav = nv) {
navRef = vessel->GetSurfaceRef();
navType = tp;
if (navRef) {
VECTOR3 npos;
NAVDATA data;
double rad;
oapiGetNavPos (nav, &npos);
oapiGlobalToEqu (navRef, npos, &navlng, &navlat, &rad);
oapiGetNavData (nav, &data);
if (navType == TRANSMITTER_ILS) crs = data.ils.appdir;
} else nav = NULL;
} else {
navType = TRANSMITTER_NONE;
}
// transform glideslope background
static float gs_tv[4] = {(horzy+171.5f)/(float)texh,(horzy+154.5f)/(float)texh,(horzy+171.5f)/(float)texh,(horzy+154.5f)/(float)texh};
vofs = vtxofs+4;
for (i = 0; i < 4; i++)
grp->Vtx[vofs+i].tv = (navType == TRANSMITTER_ILS ? gs_tv[i] : (horzy+154.5f)/(float)texh);
// transform glideslope indicator
if (navType != TRANSMITTER_ILS) {
vofs = vtxofs+8;
for (i = 0; i < 4; i++) grp->Vtx[vofs+i].y = ycnt-64;
}
}
if (nav) {
double vlng, vlat, vrad, adist;
OBJHANDLE hRef = vessel->GetEquPos (vlng, vlat, vrad);
if (hRef && hRef == navRef) {
Orthodome (vlng, vlat, navlng, navlat, adist, brg);
adist *= oapiGetSize (hRef);
dev = brg-crs;
if (dev < -PI) dev += PI2;
else if (dev >= PI) dev -= PI2;
if (dev < -PI05) dev = -PI-dev;
else if (dev >= PI05) dev = PI-dev;
// calculate slope
if (navType == TRANSMITTER_ILS) {
double s = adist * cos(crs-brg);
double alt = vessel->GetAltitude();
slope = atan2 (alt, s) * DEG;
// transform glideslope indicator
const double tgtslope = 4.0;
double dslope = slope - tgtslope;
float yshift = (float)min(fabs(dslope)*20.0,45.0);
if (dslope < 0.0) yshift = -yshift;
static float gs_y[4] = {ycnt-4.0f, ycnt-4.0f, ycnt+4.0f, ycnt+4.0f};
vofs = vtxofs+8;
for (i = 0; i < 4; i++)
grp->Vtx[vofs+i].y = gs_y[i]+yshift;
}
}
}
static double xp[4] = {-60.5,60.5,-60.5,60.5};
static double yp[4] = {-60.5,-60.5,60.5,60.5};
// transform compass rose
vofs = vtxofs;
for (i = 0; i < 4; i++) {
grp->Vtx[i+vofs].x = (float)(cosy*xp[i] + siny*yp[i] + xcnt);
grp->Vtx[i+vofs].y = (float)(-siny*xp[i] + cosy*yp[i] + ycnt);
}
// transform source bearing indicator
vofs = vtxofs+12;
if (nav) {
c = yaw-brg;
sinc = sin(c), cosc = cos(c);
static double xs[4] = {-6.2,6.2,-6.2,6.2};
static double ys[4] = {-61,-61,-45,-45};
for (i = 0; i < 4; i++) {
grp->Vtx[i+vofs].x = (float)(cosc*xs[i] + sinc*ys[i] + xcnt);
grp->Vtx[i+vofs].y = (float)(-sinc*xs[i] + cosc*ys[i] + ycnt);
}
} else { // hide indicator
for (i = 0; i < 4; i++) {
grp->Vtx[i+vofs].x = (float)(xcnt-65.0);
grp->Vtx[i+vofs].y = (float)ycnt;
}
}
// transform course indicator + scale
c = yaw-crs;
sinc = sin(c), cosc = cos(c);
static double xc[8] = {-32.2,32.2,-32.2,32.2, -6.2, 6.2, -6.2, 6.2};
static double yc[8] = { -4.7, -4.7, 4.7, 4.7,-60.5,-60.5,60.5,60.5};
vofs = vtxofs+16;
for (i = 0; i < 8; i++) {
grp->Vtx[i+vofs].x = (float)(cosc*xc[i] + sinc*yc[i] + xcnt);
grp->Vtx[i+vofs].y = (float)(-sinc*xc[i] + cosc*yc[i] + ycnt);
}
// transform deviation indicator
static double xd[4] = {-3.65,3.65,-3.65,3.65};
static double yd[4] = {-26.82,-26.82,26.82,26.82};
double dx = min(8.0,fabs(dev)*DEG)*5.175;
if (dev < 0.0) dx = -dx;
vofs = vtxofs+24;
for (i = 0; i < 4; i++) {
grp->Vtx[i+vofs].x = (float)(cosc*(xd[i]+dx) + sinc*yd[i] + xcnt);
grp->Vtx[i+vofs].y = (float)(-sinc*(xd[i]+dx) + cosc*yd[i] + ycnt);
}
// course readout
int icrs = (int)(crs*DEG+0.5) % 360;
char *cc, cbuf[16];
sprintf (cbuf, "%03d", icrs);
vofs = vtxofs+32;
static double numw = 10.0, num_ofs = horzx+1.0;
static double tu_num[4] = {0,numw/texw,0,numw/texw};
for (cc = cbuf, i = 0; i < 3; cc++, i++) {
double x = ((*cc-'0') * numw + num_ofs)/texw;
for (j = 0; j < 4; j++)
grp->Vtx[i*4+j+vofs].tu = (float)(tu_num[j]+x);
}
return false;
}
void AttitudeReference::PostStep (double simt, double simdt, double mjd)
{
valid_axes = false;
valid_euler = false;
valid_tgteuler = false;
if (mode >= 4 && tgtmode == 3) {
NAVHANDLE hNav = v->GetNavSource (navid);
if (hNav) {
VECTOR3 tvel,svel;
v->GetGlobalVel (svel);
NAVDATA data;
OBJHANDLE hObj = NULL;
oapiGetNavData (hNav, &data);
switch (data.type) {
case TRANSMITTER_IDS:
hObj = data.ids.hVessel;
break;
case TRANSMITTER_XPDR:
hObj = data.xpdr.hVessel;
break;
case TRANSMITTER_VTOL:
hObj = data.vtol.hBase;
break;
case TRANSMITTER_VOR: {
hObj = data.vor.hPlanet;
MATRIX3 Rp;
oapiGetRotationMatrix (hObj, &Rp);
oapiGetGlobalVel (hObj, &tvel);
tvel += mul (Rp, _V(-sin(data.vor.lng),0,cos(data.vor.lng)) * PI2/oapiGetPlanetPeriod(hObj)*oapiGetSize(hObj)*cos(data.vor.lat));
tgt_rvel = svel-tvel;
sprintf (oapiDebugString(), "rvel: x=%f, y=%f, z=%f", tgt_rvel.x, tgt_rvel.y, tgt_rvel.z);
} return; // done
}
if (hObj) {
oapiGetGlobalVel (hObj, &tvel);
tgt_rvel = svel-tvel;
} else {
// TODO
}
}
}
}
