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; } }