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