void SyncRefsDir(const char *dir, const String& rel, Progress& pi) { SyncRefsRunning++; for(FindFile pff(AppendFileName(dir, "*.*")); pff && !IdeExit; pff.Next()) { if(pff.IsFolder() && *pff.GetName() != '.') { if(SyncRefsShowProgress) pi.Step(); TopicLink tl; tl.package = rel + pff.GetName(); String pdir = AppendFileName(dir, pff.GetName()); for(FindFile ff(AppendFileName(pdir, "*.tpp")); ff && !IdeExit; ff.Next()) { if(ff.IsFolder()) { String group = GetFileTitle(ff.GetName()); tl.group = group; String dir = AppendFileName(pdir, ff.GetName()); LSLOW(); for(FindFile ft(AppendFileName(dir, "*.tpp")); ft && !IdeExit; ft.Next()) { if(ft.IsFile()) { String path = AppendFileName(dir, ft.GetName()); tl.topic = GetFileTitle(ft.GetName()); String link = TopicLinkString(tl); if(SyncRefsShowProgress) pi.SetText("Indexing topic " + tl.topic); SyncTopicFile(link); } if(!SyncRefsFinished && !SyncRefsShowProgress && !IdeExit) Ctrl::ProcessEvents(); } } } SyncRefsDir(pdir, tl.package + '/', pi); } } SyncRefsRunning--; }
void GatherTpp::GatherRefLinks(const char *upp) { for(FindFile pff(AppendFileName(upp, "*.*")); pff; pff.Next()) { if(pff.IsFolder()) { String package = pff.GetName(); String pdir = AppendFileName(upp, package); TopicLink tl; tl.package = package; for(FindFile ff(AppendFileName(pdir, "*.tpp")); ff; ff.Next()) { if(ff.IsFolder()) { String group = GetFileTitle(ff.GetName() ); tl.group = group; String dir = AppendFileName(pdir, ff.GetName()); for(FindFile ft(AppendFileName(dir, "*.tpp")); ft; ft.Next()) { if(ft.IsFile()) { String path = AppendFileName(dir, ft.GetName()); tl.topic = GetFileTitle(ft.GetName()); String link = TopicLinkString(tl); ScanTopicIterator sti(&reflink); sti.link = link; ParseQTF(ReadTopic(LoadFile(path))).Iterate(sti); } } } } } } }
bool tPropSurface::openPff(QString fileName, tLayer* hostingLayer) { setElementLocked clear(); int i; bool ok[3]; tReal SIscale = 0.001; // Konvertierung von mm in m tList<tElement*> sectionL, radialsL, radialsSS, radialsPS, pitchL, skewL, rakeL, wakeL, chordL, camberL, camberCurveL, thickL, thickCurveL, maxThickL, pointL, shapeL; tReal rR, chord, pitch, distLE, rake, thickMax, thick, camber, t, xSS, xPS; tVector xx; tCSpline *pitchC, *chordC, *rakeC, *skewC, *maxThickC; tCLoft *thickS, *camberS; tPoint *point; tCSpline *cspl; QString line; QStringList Vector; QFile pff(fileName); QFileInfo pffInfo(pff); fpffFileName = fileName; if (!pff.open(QIODevice::ReadOnly | QIODevice::Text)) { invalidate(this); return false; } else { // pff-Daten einlesen char dbg[500]; strcpy(dbg,fileName.toLatin1().data()); QTextStream in(&pff); do { line = in.readLine(); strcpy(dbg,line.toLatin1().data()); } while (!line.contains("PropDiameter", Qt::CaseInsensitive)); Vector = in.readLine().split(QRegExp("\\s+"),QString::SkipEmptyParts); fScale = Vector[2].toFloat(); if (fScale <= 0.){ fScale = 1.; } fDiameter = fabs(Vector[0].toFloat()*SIscale); if (fDiameter == 0.){ fDiameter = 1.; } fhubDiameter = fabs(Vector[1].toFloat()*SIscale)/fDiameter; for (i=0;i<3;i++){ in.readLine(); } Vector = in.readLine().split(QRegExp("\\s+"),QString::SkipEmptyParts); fZ = Vector[0].toInt(); if (Vector[4].toInt() == 1){ //Drehsinn frotationDirection = true; //rechts drehend } else { frotationDirection = false; //links drehend } line = in.readLine(); sectionL.clear(); while (line.contains("r/R")){ Vector = in.readLine().split(QRegExp("\\s+"),QString::SkipEmptyParts); rR = Vector[0].toFloat(); // r = rR*fDiameter/2.; // r = Vector[1].toFloat()*fScale; chord = Vector[2].toFloat()*SIscale; in.readLine(); Vector = in.readLine().split(QRegExp("\\s+"),QString::SkipEmptyParts); pitch = Vector[0].toFloat()*SIscale; distLE = Vector[1].toFloat()*SIscale; rake = Vector[4].toFloat()*SIscale; in.readLine(); line = in.readLine(); t = -1.; thickMax = 0.; thickL.clear(); camberL.clear(); while (!line.contains("*")){ Vector = line.split(QRegExp("\\s+"), QString::SkipEmptyParts); if (Vector.count() > 2 && Vector[0].toFloat()-t>1e-7){ t = Vector[0].toFloat(ok); xSS = Vector[1].toFloat(ok+1)*SIscale; //Punkt auf der Saugseite xPS = Vector[2].toFloat(ok+2)*SIscale; //Punkt auf der Druckseite if (ok[0] && ok[1] && ok[2]){ thick = fabs(xSS-xPS); thickMax = max(thick,thickMax); if (thickL.count() == 0){ thickL.append(new tPoint(NULL,tVector(rR,t,0.))); // Die Eintrittskante soll eine Dicke von 0 haben. } else { // thickL.append(new tPoint(NULL,tVector(rR,t,0.))); thickL.append(new tPoint(NULL,tVector(rR,t,thick))); } camber = (xSS+xPS)*0.5; // camberL.append(new tPoint(NULL,tVector(rR,t,sin(t*M_PI)*0.01))); // camberL.append(new tPoint(NULL,tVector(rR,t,camber/fDiameter*10.))); camberL.append(new tPoint(NULL,tVector(rR,t,camber/fDiameter))); } } line = in.readLine(); } thickL.reverse(); for (i=thickL.count()-2;i>=0;i--){ xx = ((tPoint*)(thickL.at(i)))->vector(); xx.z *= -1; thickL.append(new tPoint(this, xx)); } thickL.reverse(); if (thickMax != 0){ for (i=0;i<thickL.count();i++){ point = dynamic_cast<tPoint*>(thickL.at(i)); if (point){ xx = point->vector(); xx.z /= thickMax; point->setVector(xx); } } } if (thickMax > 0){ cspl = new tCSpline(NULL,&thickL); cspl->setCSplineType(jrCSLengthBased); thickCurveL.append(cspl); } else { for (i=0;i<thickL.count();i++){ thickL.at(i)->releaseOwner(this); } } cspl = new tCSpline(NULL,&camberL); cspl->setCSplineType(jrCSLengthBased); camberCurveL.append(cspl); pitchL.append(new tPoint(NULL, tVector(rR,pitch/fDiameter,0.))); chordL.append(new tPoint(NULL, tVector(rR,chord/fDiameter,0.))); rakeL.append(new tPoint(NULL, tVector(rR,rake/fDiameter,0.))); skewL.append(new tPoint(NULL, tVector(rR, cos(atan(pitch/fDiameter/M_PI))*(chord/2.-distLE)/(M_PI*rR*fDiameter)*(float)fZ, 0.))); maxThickL.append(new tPoint(NULL, tVector(rR,thickMax/fDiameter,0.))); } // pff-Daten einlesen beendet. // Basiskurven erzeugen; if (hostingLayer){ pitchC = hostingLayer->addCSpline(); chordC = hostingLayer->addCSpline(); skewC = hostingLayer->addCSpline(); rakeC = hostingLayer->addCSpline(); maxThickC = hostingLayer->addCSpline(); thickS = hostingLayer->addCLoft(); camberS = hostingLayer->addCLoft(); pitchC->intrface()->setVisibility(false); chordC->intrface()->setVisibility(false); skewC->intrface()->setVisibility(false); maxThickC->intrface()->setVisibility(false); rakeC->intrface()->setVisibility(false); thickS->intrface()->setVisibility(false); camberS->intrface()->setVisibility(false); } else { pitchC = new tCSpline(NULL); chordC = new tCSpline(NULL); skewC = new tCSpline(NULL); maxThickC = new tCSpline(NULL); rakeC = new tCSpline(NULL); thickS = new tCLoft(NULL); camberS = new tCLoft(NULL); } pitchC->intrface()->setName(intrface()->name() + ".pitch"); chordC->intrface()->setName(intrface()->name() + ".chord"); skewC->intrface()->setName(intrface()->name() + ".skew"); maxThickC->intrface()->setName(intrface()->name() + ".maximumThickness"); rakeC->intrface()->setName(intrface()->name() + ".rake"); thickS->intrface()->setName(intrface()->name() + ".thickness"); camberS->intrface()->setName(intrface()->name() + ".camber"); setPitch(pitchC); setChord(chordC); setSkew(skewC); setMaxThick(maxThickC); setRake(rakeC); setThickness(thickS); setCamber(camberS); // pitch ist nicht lengthbased chordC->setCSplineType(jrCSLengthBased); skewC->setCSplineType(jrCSLengthBased); maxThickC->setCSplineType(jrCSLengthBased); rakeC->setCSplineType(jrCSLengthBased); // chordC->insertList(0,&chordL); if (fRoundedTip){ tList<tElement*> list; list = chordL; chordL.clear(); for (i=list.count()-1; i>=0; i--){ xx = ((tPoint*)list.at(i))->vector(); chordL.prepend(new tPoint(NULL, xx)); if (fRoundedTip == true){ // chordSpiegeln: xx.y *= -1; chordL.append(new tPoint(NULL, xx)); } } } chordC->insertList(0,&chordL); pitchC->insertList(0,&pitchL); rakeC->insertList(0,&rakeL); skewC->insertList(0,&skewL); maxThickC->insertList(0,&maxThickL); camberS->insertList(0,&camberCurveL); camberS->setUDiv(camberCurveL.count()); camberS->setURes(20); thickS->insertList(0,&thickCurveL); thickS->setUDiv(thickCurveL.count()); thickS->setURes(20); invalidate(this); return true; } }
void des_thrusters::step(double delta) { bool softkilled; shm_get(switches, soft_kill, softkilled); shm_getg(settings_control, shm_settings); if (softkilled || !shm_settings.enabled) { f -= f; t -= t; return; } // Read PID settings & desires. // (would switching to watchers improve performance?) SHM_GET_PID(settings_heading, shm_hp, hp) SHM_GET_PID(settings_pitch, shm_pp, pp) SHM_GET_PID(settings_roll, shm_rp, rp) SHM_GET_PID(settings_velx, shm_xp, xp) SHM_GET_PID(settings_vely, shm_yp, yp) SHM_GET_PID(settings_depth, shm_dp, dp) shm_getg(desires, shm_desires); // Read current orientation, velocity & position (in the model frame). // Orientation quaternion in the model frame. Eigen::Quaterniond qm = q * mtob_rq; Eigen::Vector3d rph = quat_to_euler(qm); // Component of the model quaternion about the z-axis (heading-only rotation). Eigen::Quaterniond qh = euler_to_quat(rph[2], 0, 0); // Velocity in heading modified world space. Eigen::Vector3d vp = qh.conjugate() * v; // Step the PID loops. Eigen::Vector3d desires = quat_to_euler(euler_to_quat(shm_desires.heading * DEG_TO_RAD, shm_desires.pitch * DEG_TO_RAD, shm_desires.roll * DEG_TO_RAD)) * RAD_TO_DEG; double ho = hp.step(delta, desires[2], rph[2] * RAD_TO_DEG); double po = pp.step(delta, desires[1], rph[1] * RAD_TO_DEG); double ro = rp.step(delta, desires[0], rph[0] * RAD_TO_DEG); double xo = xp.step(delta, shm_desires.speed, vp[0]); double yo = yp.step(delta, shm_desires.sway_speed, vp[1]); double zo = dp.step(delta, shm_desires.depth, x[2]); // f is in the heading modified world frame. // t is in the model frame. // We will work with f and b in the model frame until the end of this // function. f[0] = shm_settings.velx_active ? xo : 0; f[1] = shm_settings.vely_active ? yo : 0; f[2] = shm_settings.depth_active ? zo : 0; f *= m; Eigen::Vector3d w_in; w_in[0] = shm_settings.roll_active ? ro : 0; w_in[1] = shm_settings.pitch_active ? po : 0; w_in[2] = shm_settings.heading_active ? ho : 0; // TODO Avoid this roundabout conversion from hpr frame // to world frame and back to model frame. Eigen::Quaterniond qhp = euler_to_quat(rph[2], rph[1], 0); Eigen::Vector3d w = qm.conjugate() * (Eigen::Vector3d(0, 0, w_in[2]) + qh * Eigen::Vector3d(0, w_in[1], 0) + qhp * Eigen::Vector3d(w_in[0], 0, 0)); t = btom_rm * q.conjugate() * I * q * mtob_rm * w; // Output diagnostic information. Shown by auv-control-helm. SHM_PUT_PID(control_internal_heading, ho) SHM_PUT_PID(control_internal_pitch, po) SHM_PUT_PID(control_internal_roll, ro) SHM_PUT_PID(control_internal_velx, xo) SHM_PUT_PID(control_internal_vely, yo) SHM_PUT_PID(control_internal_depth, zo) // Subtract passive forces. // (this implementation does not support discrimination!) if (shm_settings.buoyancy_forces || shm_settings.drag_forces) { // pff is in the body frame. screw ps = pff(); f -= qh.conjugate() * q * ps.first; t -= btom_rm * ps.second; } // Hyper-ellipsoid clamping. Sort of limiting to the maximum amount of // energy the sub can output (e.g. can't move forward at full speed // and pitch at full speed at the same time). // Doesn't really account for real-world thruster configurations (e.g. // it might be possible to move forward at full speed and ascend at // full speed at the same time) but it's just an approximation. for (int i = 0; i < 3; i++) { f[i] /= axes[i]; } for (int i = 0; i < 3; i++) { t[i] /= axes[3 + i]; } double m = f.dot(f) + t.dot(t); if (m > 1) { double sm = sqrt(m); f /= sm; t /= sm; } for (int i = 0; i < 3; i++) { f[i] *= axes[i]; } for (int i = 0; i < 3; i++) { t[i] *= axes[3 + i]; } // Regular min/max clamping. clamp(f, fa, fb, 3); clamp(t, ta, tb, 3); f = q.conjugate() * qh * f; t = mtob_rm * t; }