void LEM::SeparateStage (UINT stage)
{
ResetThrusters();
VESSELSTATUS vs1;
VECTOR3 ofs1 = _V(0,-5,0);
VECTOR3 vel1 = _V(0,0,0);
if (stage == 1) {
GetStatus (vs1);
vs1.eng_main = vs1.eng_hovr = 0.0;
VECTOR3 rofs1, rvel1 = {vs1.rvel.x, vs1.rvel.y, vs1.rvel.z};
Local2Global (ofs1, vs1.rpos);
GlobalRot (vel1, rofs1);
vs1.rvel.x = rvel1.x+rofs1.x;
vs1.rvel.y = rvel1.y+rofs1.y;
vs1.rvel.z = rvel1.z+rofs1.z;
vs1.vrot.x = 0.0;
vs1.vrot.y = 0.0;
vs1.vrot.z = 0.0;
GetStatus (vs1);
// Wrong sound, is Saturn staging
// StageS.play(NOLOOP, 150);
char VName[256];
strcpy (VName, GetName()); strcat (VName, "-DESCENTSTG");
hdsc = oapiCreateVessel(VName, "ProjectApollo/sat5LMDSC", vs1);
SetLmAscentHoverStage();
}
}
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 FLTM::execute( ClustAlgoPtr clustAlgo, CardFuncPtr cardFunc, GraphPtr graph ) {
auto lab2Idx = create_index_map(*graph);
Local2GlobalPtr l2g = create_local_to_global_map(*graph);
auto criteria = clustAlgo->get_criteria();
int verticesNb = boost::num_vertices(*graph);
for ( int step = 0; step < params.nbrSteps; ++step) {
if (step > 0) {
criteria = create_current_criteria( *graph, *l2g, params.maxDist, step);
clustAlgo->set_measure( graph, l2g, criteria );
}
BOOST_LOG_TRIVIAL(trace) << "FLTM - step[" << step << "] over " << params.nbrSteps;
BOOST_LOG_TRIVIAL(trace) << "running clustering " << clustAlgo->name()
<< " on " << l2g->size();
auto partition = clustAlgo->run();
auto clustering = partition.to_clustering();
auto SIZE = l2g->size();
int nonSingletons = number_non_singletons(clustering);
BOOST_LOG_TRIVIAL(trace) << "to obtain " << clustering.size() << " clusters with " << nonSingletons << " non-singletons clusters" ;
if ( nonSingletons == 0 ) {
BOOST_LOG_TRIVIAL(trace) << "stop due to only singleton.";
return;
}
std::vector<int> l2gTemp(*l2g);
Local2Global().swap(*l2g);
int nbrGoodClusters = 0;
// loop without any parallelization
// for ( auto &cluster: clustering ) {
// if ( cluster.size() > 1 ) {
// //numClust++;
// RandVar var("latent-"+boost::lexical_cast<std::string>(boost::num_vertices(*graph)),
// plIntegerType(0, cardFunc->compute(cluster) - 1 ));
// Node latentNode = create_latent_node( graph, var, l2gTemp, lab2Idx, cluster);
// MultiEM em(params.nbrRestarts);
// em.run( *graph, latentNode, params.emThres);
// if ( accept_latent_variable( *graph, latentNode, params.latentVarQualityThres) ) {
// nbrGoodClusters++;
// add_latent_node( *graph, latentNode );
// update_index_map( *l2g, l2gTemp, latentNode );
// lab2Idx[ latentNode.getLabel() ] = latentNode.index;
// for ( auto item: cluster ) {
// // l2g.push_back( currentL2G.at(item) );
// boost::add_edge( latentNode.index, l2gTemp.at(item), *graph);
// }
// } else {
// update_index_map( *l2g, l2gTemp, cluster);
// }
// } else {
// update_index_map( *l2g, l2gTemp, cluster);
// }
// }
// loop with working parallelization
#ifdef _OPENMP
//sets the max number of threads we can use
omp_set_num_threads(params.jobsNumber);
#endif
//the array of shared resources in which the differents threads write
Node latentVector[nonSingletons];
//the parallelizable section
#pragma omp parallel for schedule(dynamic)
for ( int i = 0 ; i < clustering.size() ; ++i) {
if ( clustering[i].size() > 1 ) {
RandVar var("latent-"+std::to_string(verticesNb + i),
plIntegerType(0, cardFunc->compute(clustering[i]) - 1 ));
latentVector[i] = create_latent_node( graph, var, l2gTemp, lab2Idx, clustering[i]);
MultiEM em(params.nbrRestarts);
em.run( *graph, latentVector[i], params.emThres);
}
}
//the non parallelizable section
for ( int i = 0 ; i < clustering.size() ; ++i) {
if (clustering[i].size() > 1 && accept_latent_variable( *graph, latentVector[i], params.latentVarQualityThres)) {
nbrGoodClusters++;
add_latent_node( *graph, latentVector[i] );
update_index_map( *l2g, l2gTemp, latentVector[i] );
lab2Idx[ latentVector[i].getLabel() ] = latentVector[i].index;
for ( auto item: clustering[i] ) {
boost::add_edge( latentVector[i].index, l2gTemp.at(item), *graph);
}
} else {
update_index_map( *l2g, l2gTemp, clustering[i]);
}
}
verticesNb += nonSingletons ;
// loop with parallelization slower than over
// #pragma omp parallel for schedule(static)
// for ( auto cluster = clustering.begin(); cluster < clustering.end() ; ++cluster) {
// //for ( auto &cluster: clustering ) {
// if ( cluster->size() > 1 ) {
// RandVar var("latent-"+boost::lexical_cast<std::string>(boost::num_vertices(*graph)),
// plIntegerType(0, cardFunc->compute(*cluster) - 1 ));
// Node latentNode = create_latent_node( graph, var, l2gTemp, lab2Idx, *cluster);
// MultiEM em(params.nbrRestarts);
// em.run( *graph, latentNode, params.emThres);
// if ( accept_latent_variable( *graph, latentNode, params.latentVarQualityThres) ) {
// #pragma omp critical
// {
// nbrGoodClusters++;
// add_latent_node( *graph, latentNode );
// update_index_map( *l2g, l2gTemp, latentNode );
// lab2Idx[ latentNode.getLabel() ] = latentNode.index;
// for ( auto item: *cluster ) {
// // l2g.push_back( currentL2G.at(item) );
// boost::add_edge( latentNode.index, l2gTemp.at(item), *graph);
// }
// }
// } else {
// #pragma omp critical
// {
// update_index_map( *l2g, l2gTemp, *cluster);
// }
// }
// } else {
// #pragma omp critical
// {
// update_index_map( *l2g, l2gTemp, *cluster);
// }
// }
// }
BOOST_LOG_TRIVIAL(trace) << "nbrGoodClusters: " << nbrGoodClusters;
if ( nbrGoodClusters == 0 ) {
BOOST_LOG_TRIVIAL(trace) << "stop due to zero good clusters.";
return;
}
if (l2g->size() <= 1) {
BOOST_LOG_TRIVIAL(trace) << "stop due to zero or only one cluster.";
return;
}
BOOST_LOG_TRIVIAL(trace) << std::endl << std::endl;
}
}
