bool ElevatorTrim::ProcessMouse2D (int event, int mx, int my)
{
double tgtlvl = vessel->GetControlSurfaceLevel (AIRCTRL_ELEVATORTRIM);
tgtlvl += oapiGetSimStep() * (my < 30 ? -0.2:0.2);
tgtlvl = max (-1.0, min (1.0, tgtlvl));
vessel->SetControlSurfaceLevel (AIRCTRL_ELEVATORTRIM, tgtlvl);
return 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);
}
bool ADIBall::Redraw2D (SURFHANDLE surf)
{
VECTOR3 euler = aref->GetEulerAngles ();
double rho = euler.x; // roll angle
double tht = euler.y; // pitch angle
double phi = euler.z; // yaw angle
double dt = oapiGetSimStep();
const double ballspeed = 3.0;
double dangle_max = ballspeed*dt;
double drho = rho-rho_curr;
if (drho > PI) drho -= PI2;
else if (drho < -PI) drho += PI2;
double dtht = tht-tht_curr;
if (dtht > PI) dtht -= PI2;
else if (dtht < -PI) dtht += PI2;
double dphi = phi-phi_curr;
if (dphi > PI) dphi -= PI2;
else if (dphi < -PI) dphi += PI2;
double dangle = max (fabs(drho), max (fabs(dtht), fabs(dphi)));
if (dangle > dangle_max) {
double scale = dangle_max/dangle;
rho = rho_curr + drho*scale;
tht = tht_curr + dtht*scale;
phi = phi_curr + dphi*scale;
}
rho_curr = rho;
tht_curr = tht;
phi_curr = phi;
DWORD i;
double sinp = sin(phi), cosp = cos(phi);
double sint = sin(tht), cost = cos(tht);
double sinr = sin(rho), cosr = cos(rho);
// Ball transformation
double a1, b1, c1, a2, b2, c2, a3, b3, c3;
if (layout == 0) {
// below are the coefficients of the rows of the rotation matrix M for the ball,
// given by M = RTP, with
// MATRIX3 P = {cosp,0,-sinp, 0,1,0, sinp,0,cosp};
// MATRIX3 T = {1,0,0, 0,cost,-sint, 0,sint,cost};
// MATRIX3 R = {cosr,sinr,0, -sinr,cosr,0, 0,0,1};
a1 = cosr*cosp - sinr*sint*sinp;
b1 = sinr*cost;
c1 = -cosr*sinp - sinr*sint*cosp;
a2 = -sinr*cosp - cosr*sint*sinp;
b2 = cosr*cost;
c2 = sinr*sinp - cosr*sint*cosp;
a3 = cost*sinp;
b3 = sint;
c3 = cost*cosp;
} else {
// below are the coefficients of the rows of the rotation matrix M for the ball,
// given by M = ZRPT, with
// MATRIX3 Z = {0,1,0, -1,0,0, 0,0,1};
// MATRIX3 P = {1,0,0, 0,cosp,-sinp, 0,sinp,cosp}; // yaw
// MATRIX3 T = {cost,0,sint, 0,1,0, -sint,0,cost}; // pitch
// MATRIX3 R = {cosr,sinr,0, -sinr,cosr,0, 0,0,1}; // bank
a1 = -sinr*cost + cosr*sinp*sint;
b1 = cosr*cosp;
c1 = -sinr*sint - cosr*sinp*cost;
a2 = -cosr*cost - sinr*sinp*sint;
b2 = -sinr*cosp;
c2 = -cosr*sint + sinr*sinp*cost;
a3 = -cosp*sint;
b3 = sinp;
c3 = cosp*cost;
}
for (i = 0; i < nballvtx; i++) {
ballgrp->Vtx[ballofs+i].x = bb_cntx + (float)(a1*ballvtx0[i].x + b1*ballvtx0[i].y + c1*ballvtx0[i].z);
ballgrp->Vtx[ballofs+i].y = bb_cnty - (float)(a2*ballvtx0[i].x + b2*ballvtx0[i].y + c2*ballvtx0[i].z);
}
// Roll indicator transformation
static const float bx[4] = {-bank_dx2,bank_dx2,-bank_dx2,bank_dx2};
static const float by[4] = {-bank_rad,-bank_rad,-bank_rad+bank_dy,-bank_rad+bank_dy};
for (i = 0; i < 4; i++) {
indgrp->Vtx[rollindofs+i].x = bb_cntx + (float)(bx[i]*cosr-by[i]*sinr);
indgrp->Vtx[rollindofs+i].y = bb_cnty + (float)(bx[i]*sinr+by[i]*cosr);
}
// error needles
double tgtx, tgty;
static const float yexofs[4] = {bb_cntx-needle_w2,bb_cntx+needle_w2,bb_cntx-needle_w2,bb_cntx+needle_w2};
static const float peyofs[4] = {bb_cnty+needle_w2,bb_cnty-needle_w2,bb_cnty+needle_w2,bb_cnty-needle_w2};
const float erange = 42.0f;
int tgtflag;
VECTOR3 euler_tgt;
if (!aref->GetTgtEulerAngles (euler_tgt)) {
tgtx = tgty = 0.0;
tgtflag = 0;
} else {
VECTOR3 tgt;
double sint = sin(euler_tgt.y), cost = cos(euler_tgt.y);
double sinp = sin(euler_tgt.z), cosp = cos(euler_tgt.z);
if (layout == 0) {
tgt = _V(rad*sinp*cost, rad*sint, rad*cosp*cost);
} else {
tgt = _V(-rad*sint*cosp, rad*sinp, rad*cost*cosp);
}
tgtx = min(max( (a1*tgt.x + b1*tgt.y + c1*tgt.z), -erange), erange);
tgty = min(max(-(a2*tgt.x + b2*tgt.y + c2*tgt.z), -erange), erange);
double tgtz = a3*tgt.x + b3*tgt.y + c3*tgt.z;
tgtflag = (tgtz >= 0.0 ? 1 : 2);
}
const double needlespeed = 50.0;
double dneedlemax = needlespeed*dt;
if (fabs(tgtx-tgtx_curr) > dneedlemax)
tgtx = (tgtx > tgtx_curr ? tgtx_curr + dneedlemax : tgtx_curr - dneedlemax);
if (fabs(tgty-tgty_curr) > dneedlemax)
tgty = (tgty > tgty_curr ? tgty_curr + dneedlemax : tgty_curr - dneedlemax);
tgtx_curr = tgtx;
tgty_curr = tgty;
for (i = 0; i < 4; i++) {
indgrp->Vtx[yeofs+i].x = (float)tgtx+yexofs[i];
indgrp->Vtx[peofs+i].y = (float)tgty+peyofs[i];
}
// to/from indicator
static float tf_tu[4] = {tx_tf_x0/texw,(tx_tf_x0+tx_tf_dx)/texw,tx_tf_x0/texw,(tx_tf_x0+tx_tf_dx)/texw};
float tf_dtu = tgtflag * tx_tf_dx/texw;
for (i = 0; i < 4; i++)
indgrp->Vtx[tfofs+i].tu = tf_tu[i] + tf_dtu;
// Rate indicators
const double rate_scale = DEG*4.0;
const double rate_max = 41.0;
if (rate_local) {
vessel->GetAngularVel (vrot);
} else {
double t = oapiGetSimTime();
if (t > euler_t) {
VECTOR3 de = (euler-peuler)/(t-euler_t);
vrot.x = de.y;
vrot.y = -de.z;
vrot.z = -de.x;
peuler = euler;
euler_t = t;
}
}
// Pitch rate indicator transformation
static const float pry[4] = {bb_cnty+rate_w2,bb_cnty-rate_w2,bb_cnty+rate_w2,bb_cnty-rate_w2};
float dp = (float)max (-rate_max, min (rate_max, vrot.x*rate_scale));
for (i = 0; i < 4; i++) indgrp->Vtx[prateofs+i].y = pry[i]-dp;
// Roll rate indicator transformation
static const float brx[4] = {bb_cntx+rate_w2,bb_cntx-rate_w2,bb_cntx+rate_w2,bb_cntx-rate_w2};
float db = (float)max (-rate_max, min (rate_max, vrot.z*rate_scale));
for (i = 0; i < 4; i++) indgrp->Vtx[brateofs+i].x = brx[i]+db;
// Yaw rate indicator transformation
static const float yrx[4] = {bb_cntx-rate_w2,bb_cntx+rate_w2,bb_cntx-rate_w2,bb_cntx+rate_w2};
float dy = (float)max (-rate_max, min (rate_max, vrot.y*rate_scale));
for (i = 0; i < 4; i++) indgrp->Vtx[yrateofs+i].x = yrx[i]-dy;
return false;
}
void VOBJ::TimeStep(int SoundID)
{
if (!oapiIsVessel(hook)){
sprintf(msg, "Unknown Error");
return;
}
if (!oapiGetVesselByIndex(iTarget) && mode == VTOV){
mode=MAGIC;
iTargetSource=0;
iTarget=0;
}
if (VESSEL(hook).GetPropellantCount() < 1)
{
sprintf(msg, "No fuel sources available");
return;
}
if (status==IDLE)
{
sprintf(msg, "IDLE: Waiting for action");
return;
}
PlayMFDWave(SoundID, 0);
if (mode==VTOV)
{
VESSEL * vHook = oapiGetVesselInterface(hook);
VESSEL * vTarget = oapiGetVesselInterface(oapiGetVesselByIndex(iTarget));
if (vTarget->GetPropellantCount() < 1)
{
sprintf(msg, "Target has no fuel sources");
return;
}
PROPELLANT_HANDLE hpTarget = vTarget->GetPropellantHandleByIndex(iTargetSource);
PROPELLANT_HANDLE hpHook = vHook->GetPropellantHandleByIndex(iSelectedSource);
if (!hpHook||!hpTarget){
sprintf(msg, "Unknown Error!");
return;
}
double maxMasstarget = vTarget->GetPropellantMaxMass(hpTarget);
double massTarget = vTarget->GetPropellantMass(hpTarget);
double maxMasshook = vHook->GetPropellantMaxMass(hpHook);
double massHook = vHook->GetPropellantMass(hpHook);
if (massTarget<CUT_OFF)
{
sprintf(msg, "No fuel left to transfer!");
return;
}
if (fabs(maxMasshook-massHook) < CUT_OFF)
{
sprintf(msg, "Tank full");
return;
}
if (hook == oapiGetVesselByIndex(iTarget))
{
if (iSelectedSource == iTargetSource)
{
sprintf(msg, "Cannot transfer fuel to the same source");
return;
}
}
double dFuelTrans = 0;
if (massTarget-dTransferRate >= 0 && massHook+dTransferRate <= maxMasshook)
{
dFuelTrans = dTransferRate;
}else
{
if (maxMasshook-massHook < massTarget)
{
dFuelTrans = maxMasshook-massHook;
}else if (maxMasshook-massHook > massTarget)
{
dFuelTrans = massTarget;
}else
dFuelTrans = massTarget;
}
dFuelTrans*=oapiGetSimStep();
vTarget->SetPropellantMass(hpTarget, massTarget-dFuelTrans);
vHook->SetPropellantMass(hpHook, massHook+dFuelTrans);
sprintf(msg, "Transfering %.2f%%", ((massHook+dFuelTrans)/maxMasshook)*100,
vTarget->GetName(), ((massTarget-dFuelTrans)/maxMasstarget)*100);
}
else if (mode == MAGIC)
{
VESSEL * v = oapiGetVesselInterface(hook);
PROPELLANT_HANDLE hpr = v->GetPropellantHandleByIndex(iSelectedSource);
if (!hpr)
{
sprintf(msg, "Unknown error!");
return;
}
double mass = v->GetPropellantMass(hpr);
double maxmass = v->GetPropellantMaxMass(hpr);
if (fabs(maxmass-mass) < CUT_OFF){
sprintf(msg, "Tank full");
return;
}
double dFuelTrans = 0;
if (mass+dTransferRate > maxmass)
dFuelTrans = maxmass-mass;
else
dFuelTrans = dTransferRate;
dFuelTrans*=oapiGetSimStep();
v->SetPropellantMass(hpr, dFuelTrans+mass);
sprintf(msg, "Transfering %.2f%%", ((mass+dFuelTrans)/maxmass)*100);
}else if (mode == DUMPING)
{
VESSEL * v = oapiGetVesselInterface(hook);
PROPELLANT_HANDLE hpr = v->GetPropellantHandleByIndex(iSelectedSource);
if (!hpr)
{
sprintf(msg, "Unknown error!");
return;
}
double mass = v->GetPropellantMass(hpr);
double maxmass = v->GetPropellantMaxMass(hpr);
if (mass <CUT_OFF)
{
sprintf(msg, "No more fuel left");
return;
}
double dFuelTrans=0;
if (mass-dTransferRate < 0)
dFuelTrans = mass;
else
dFuelTrans = dTransferRate;
dFuelTrans*=oapiGetSimStep();
v->SetPropellantMass(hpr, mass-dFuelTrans);
sprintf(msg, "Dumping %.2f%%", ((mass-dFuelTrans)/maxmass)*100);
}
}
