When we are asked to produce no force on a Surface (e.g. a weight that

[flightgear.git] / src / FDM / YASim / Rotor.cpp

diff --git a/src/FDM/YASim/Rotor.cpp b/src/FDM/YASim/Rotor.cpp
index a141be262a5cae161c5cb082711008aade76f12d..142ff75eb942ec404e5d4539eacc6d8a24ab5a08 100644 (file)
--- a/src/FDM/YASim/Rotor.cpp
+++ b/src/FDM/YASim/Rotor.cpp
@@ -1,14 +1,25 @@
-\v#include "Math.hpp"
+#include <simgear/debug/logstream.hxx>
+
+#include "Math.hpp"
 #include "Surface.hpp"
 #include "Rotorpart.hpp"
 #include "Rotorblade.hpp"
 #include "Rotor.hpp"
-#include <stdio.h>
+
+#include STL_IOSTREAM
+#include STL_IOMANIP
+
+SG_USING_STD(setprecision);
+
 //#include <string.h>
+#include <stdio.h>
+
 namespace yasim {
 
 const float pi=3.14159;
 
+static inline float sqr(float a) { return a * a; }
+
 Rotor::Rotor()
 {
     _alpha0=-.05;
@@ -81,7 +92,7 @@ void Rotor::inititeration(float dt)
 {
    if ((_engineon)&&(_omegarel>=1)) return;
    if ((!_engineon)&&(_omegarel<=0)) return;
-   _omegarel+=dt*1/5.*(_engineon?1:-1); //hier 30
+   _omegarel+=dt*1/30.*(_engineon?1:-1);
    _omegarel=Math::clamp(_omegarel,0,1);
    _omega=_omegan*_omegarel;
    int i;
@@ -209,8 +220,8 @@ int Rotor::getValueforFGSet(int j,char *text,float *f)
        rk=l-p;
        rl=1-rk;
        /*
-       rl=Math::sqr(Math::sin(rl*pi/2));
-       rk=Math::sqr(Math::sin(rk*pi/2));
+       rl=sqr(Math::sin(rl*pi/2));
+       rk=sqr(Math::sin(rk*pi/2));
        */
        if(w==2) {k+=2;l+=2;}
        else
@@ -512,11 +523,11 @@ void Rotor::compile()
 
   //rotor is divided into 4 pointlike parts
 
-  printf("debug: e %f...%f a%f...%f %f...%f\n",
-      _mincyclicele,_maxcyclicele,
-      _mincyclicail,_maxcyclicail,
-      _min_pitch,_max_pitch);
-
+  SG_LOG(SG_FLIGHT, SG_DEBUG, "debug: e "
+         << _mincyclicele << "..." <<_maxcyclicele << ' '
+         << _mincyclicail << "..." << _maxcyclicail << ' '
+         << _min_pitch << "..." << _max_pitch);
+  
   if(!_sim_blades)
   {
     _dynamic=_dynamic*(1/                          //inverse of the time
@@ -528,7 +539,7 @@ void Rotor::compile()
     directions[0][1]=_forward[1];
     directions[0][2]=_forward[2];
     int i;
-    printf("Rotor rotating ccw? %i\n",_ccw);
+    SG_LOG(SG_FLIGHT, SG_DEBUG, "Rotor rotating ccw? " << _ccw);
     for (i=1;i<5;i++)
 
     {
@@ -556,7 +567,7 @@ void Rotor::compile()
 
     float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
     //float relamp=omega*omega/(2*_delta*Math::sqrt(omega0*omega0-_delta*_delta));
-    float relamp=omega*omega/(2*_delta*Math::sqrt(Math::sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega));
+    float relamp=omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega));
     if (!_no_torque)
     {
        torque0=_power_at_pitch_0/4*1000/omega;
@@ -570,14 +581,24 @@ void Rotor::compile()
 
     }
 
-    printf("spd: %5.3f lentoc: %5.3f dia: %5.3f rbl: %5.3f hing: %5.3f lfa:%5.3f\n"
-           "zf: %5.3f mpf: %5.3f\n"
-           "tq: %5.3f..%5.3f d3:%5.3f\n"
-           "o/o0: %5.3f phi: %5.3f relamp: %5.3f delta:%5.3f\n"
-           ,speed,lentocenter,_diameter,_rel_blade_center,_rel_len_hinge,
-           lentoforceattac,zentforce,maxpitchforce,
-           torque0,torquemax,_delta3,
-           omega/omega0,phi*180/pi,relamp,_delta);
+    SG_LOG(SG_FLIGHT, SG_DEBUG,
+           "spd: " << setprecision(8) << speed
+           << " lentoc: " << lentocenter
+           << " dia: " << _diameter
+           << " rbl: " << _rel_blade_center
+           << " hing: " << _rel_len_hinge
+           << " lfa: " << lentoforceattac);
+    SG_LOG(SG_FLIGHT, SG_DEBUG,
+           "zf: " << setprecision(8) << zentforce
+           << " mpf: " << maxpitchforce);
+    SG_LOG(SG_FLIGHT, SG_DEBUG,
+           "tq: " << setprecision(8) << torque0 << ".." << torquemax
+           << " d3: " << _delta3);
+    SG_LOG(SG_FLIGHT, SG_DEBUG,
+           "o/o0: " << setprecision(8) << omega/omega0
+           << " phi: " << phi*180/pi
+           << " relamp: " << relamp
+           << " delta: " <<_delta);
 
     Rotorpart* rps[4];
     for (i=0;i<4;i++)
@@ -609,7 +630,7 @@ void Rotor::compile()
     for (i=0;i<4;i++)
     {
       
-      rps[i]->setlastnextrp(rps[(i-1)%4],rps[(i+1)%4],rps[(i+2)%4]);
+      rps[i]->setlastnextrp(rps[(i+3)%4],rps[(i+1)%4],rps[(i+2)%4]);
     }
   }
   else
@@ -619,7 +640,7 @@ void Rotor::compile()
     directions[0][1]=_forward[1];
     directions[0][2]=_forward[2];
     int i;
-    printf("Rotor rotating ccw? %i\n",_ccw);
+    SG_LOG(SG_FLIGHT, SG_DEBUG, "Rotor rotating ccw? " <<_ccw);
     for (i=1;i<5;i++)
 
     {
@@ -644,8 +665,8 @@ void Rotor::compile()
     //_delta=omega*_delta;
     _delta*=maxpitchforce/(_max_pitch*omega*lentocenter*2*_weight_per_blade*.453);
     float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
-    float phi2=Math::abs(omega0-omega)<.000000001?pi/2:Math::atan(2*omega*_delta/(omega0*omega0-omega*omega));
-    float relamp=omega*omega/(2*_delta*Math::sqrt(Math::sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega));
+    // float phi2=Math::abs(omega0-omega)<.000000001?pi/2:Math::atan(2*omega*_delta/(omega0*omega0-omega*omega));
+    float relamp=omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega));
     if (!_no_torque)
     {
        torque0=_power_at_pitch_0/_number_of_blades*1000/omega;
@@ -659,16 +680,27 @@ void Rotor::compile()
 
     }
 
-    printf("spd: %5.3f lentoc: %5.3f dia: %5.3f rbl: %5.3f hing: %5.3f lfa:%5.3f\n"
-           "zf: %5.3f mpf: %5.3f\n"
-           "tq: %5.3f..%5.3f d3:%5.3f\n"
-           "o/o0: %5.3f phi: %5.3f relamp:%5.3f delta:%5.3f\n"
-           ,speed,lentocenter,_diameter,_rel_blade_center,_rel_len_hinge,
-           lentoforceattac,zentforce,maxpitchforce,
-           torque0,torquemax,_delta3,
-           omega/omega0,float(phi*180/pi),relamp,_delta);
-
-    float lspeed[3],dirzentforce[3];
+    SG_LOG(SG_FLIGHT, SG_DEBUG,
+           "spd: " << setprecision(8) << speed
+           << " lentoc: " << lentocenter
+           << " dia: " << _diameter
+           << " rbl: " << _rel_blade_center
+           << " hing: " << _rel_len_hinge
+           << " lfa: " << lentoforceattac);
+    SG_LOG(SG_FLIGHT, SG_DEBUG,
+           "zf: " << setprecision(8) << zentforce
+           << " mpf: " << maxpitchforce);
+    SG_LOG(SG_FLIGHT, SG_DEBUG,
+           "tq: " << setprecision(8) << torque0 << ".." << torquemax
+           << " d3: " << _delta3);
+    SG_LOG(SG_FLIGHT, SG_DEBUG,
+           "o/o0: " << setprecision(8) << omega/omega0
+           << " phi: " << phi*180/pi
+           << " relamp: " << relamp
+           << " delta: " <<_delta);
+
+    // float lspeed[3];
+    float dirzentforce[3];
 
     float f=(!_ccw)?1:-1;
     //Math::mul3(f*speed,directions[1],lspeed);
@@ -790,16 +822,25 @@ Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
     r->setLen(len);
 
 
-
-    #define a(x) x[0],x[1],x[2]
-    printf("newrp: pos(%5.3f %5.3f %5.3f) pfa (%5.3f %5.3f %5.3f)\n"
-           "       nor(%5.3f %5.3f %5.3f) spd (%5.3f %5.3f %5.3f)\n"
-           "       dzf(%5.3f %5.3f %5.3f) zf  (%5.3f) mpf (%5.3f)\n"
-           "       pit (%5.3f..%5.3f) mcy (%5.3f..%5.3f) d3 (%5.3f)\n"
-           ,a(pos),a(posforceattac),a(normal),
-          a(speed),a(dirzentforce),zentforce,maxpitchforce,minpitch,maxpitch,mincyclic,maxcyclic,
-          delta3);
-    #undef a
+    SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
+           << "newrp: pos("
+           << pos[0] << ' ' << pos[1] << ' ' << pos[2]
+           << ") pfa ("
+           << posforceattac[0] << ' ' << posforceattac[1] << ' ' 
+           << posforceattac[2] << ')');
+    SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
+           << "       nor("
+           << normal[0] << ' ' << normal[1] << ' ' << normal[2]
+           << ") spd ("
+           << speed[0] << ' ' << speed[1] << ' ' << speed[2] << ')');
+    SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
+           << "       dzf("
+           << dirzentforce[0] << ' ' << dirzentforce[1] << dirzentforce[2]
+           << ") zf  (" << zentforce << ") mpf (" << maxpitchforce << ')');
+    SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
+           << "       pit(" << minpitch << ".." << maxpitch
+           << ") mcy (" << mincyclic << ".." << maxcyclic
+           << ") d3 (" << delta3 << ')');
 
     return r;
 }