1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
6 Purpose: Encapsulates the rotor object
8 ------------- Copyright (C) 2000 Jon S. Berndt (jsb@hal-pc.org) -------------
10 This program is free software; you can redistribute it and/or modify it under
11 the terms of the GNU Lesser General Public License as published by the Free Software
12 Foundation; either version 2 of the License, or (at your option) any later
15 This program is distributed in the hope that it will be useful, but WITHOUT
16 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
17 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
20 You should have received a copy of the GNU Lesser General Public License along with
21 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
22 Place - Suite 330, Boston, MA 02111-1307, USA.
24 Further information about the GNU Lesser General Public License can also be found on
25 the world wide web at http://www.gnu.org.
27 FUNCTIONAL DESCRIPTION
28 --------------------------------------------------------------------------------
31 --------------------------------------------------------------------------------
33 01/01/10 T.Kreitler test implementation
35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
44 #include "models/FGPropagate.h"
45 #include "models/FGAtmosphere.h"
46 #include "models/FGAuxiliary.h"
47 #include "models/FGMassBalance.h"
49 #include "input_output/FGXMLElement.h"
51 #include "math/FGRungeKutta.h"
57 static const char *IdSrc = "$Id: FGRotor.cpp,v 1.9 2010/06/05 12:12:34 jberndt Exp $";
58 static const char *IdHdr = ID_ROTOR;
60 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
62 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
64 static int dump_req; // debug schwafel flag
66 static inline double sqr(double x) { return x*x; }
68 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
70 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
72 // starting with 'inner' rotor, FGRotor constructor is further down
74 FGRotor::rotor::~rotor() { }
76 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
78 // hmm, not a real alternative to a pretty long initializer list
80 void FGRotor::rotor::zero() {
81 FGColumnVector3 zero_vec(0.0, 0.0, 0.0);
91 RelDistance_xhub = 0.0 ;
93 RelHeight_zhub = 0.0 ;
97 LiftCurveSlope = 0.0 ;
98 BladeFlappingMoment = 0.0 ;
100 BladeMassMoment = 0.0 ;
106 HingeOffset_hover = 0.0 ;
112 // derived parameters
113 LockNumberByRho = 0.0 ;
117 for (int i=0; i<5; i++) R[i] = 0.0;
118 for (int i=0; i<6; i++) B[i] = 0.0;
120 BodyToShaft.InitMatrix();
121 ShaftToBody.InitMatrix();
128 a_1 = b_1 = a_dw = 0.0 ;
130 H_drag = J_side = 0.0 ;
145 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
147 // 5in1: value-fetch-convert-default-return function
149 double FGRotor::rotor::cnf_elem( const string& ename, double default_val,
150 const string& unit, bool tell)
154 double val=default_val;
156 std::string pname = "*No parent element*";
159 e = parent->FindElement(ename);
160 pname = parent->GetName();
165 // val = e->FindElementValueAsNumber(ename);
166 // yields to: Attempting to get single data value from multiple lines
167 val = parent->FindElementValueAsNumber(ename);
169 // val = e->FindElementValueAsNumberConvertTo(ename,unit);
170 // yields to: Attempting to get non-existent element diameter + crash, why ?
171 val = parent->FindElementValueAsNumberConvertTo(ename,unit);
175 cerr << pname << ": missing element '" << ename <<"' using estimated value: " << default_val << endl;
182 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
184 double FGRotor::rotor::cnf_elem(const string& ename, double default_val, bool tell)
186 return cnf_elem(ename, default_val, "", tell);
189 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
191 // 1. read configuration and try to fill holes, ymmv
192 // 2. calculate derived parameters and transforms
193 void FGRotor::rotor::configure(int f, const rotor *xmain)
197 const bool yell = true;
198 const bool silent = false;
202 estimate = (xmain) ? 2.0 * xmain->Radius * 0.2 : 42.0;
203 Radius = 0.5 * cnf_elem("diameter", estimate, "FT", yell);
205 estimate = (xmain) ? xmain->BladeNum : 2.0;
206 estimate = Constrain(1.0,estimate,4.0);
207 BladeNum = (int) cnf_elem("numblades", estimate, yell);
209 estimate = (xmain) ? - xmain->Radius * 1.05 - Radius : - 0.025 * Radius ;
210 RelDistance_xhub = cnf_elem("xhub", estimate, "FT", yell);
212 RelShift_yhub = cnf_elem("yhub", 0.0, "FT", silent);
214 estimate = - 0.1 * Radius - 4.0;
215 RelHeight_zhub = cnf_elem("zhub", estimate, "FT", yell);
217 // make sure that v_tip (omega*r) is below 0.7mach ~ 750ft/s
218 estimate = (750.0/Radius)/(2.0*M_PI) * 60.0; // 7160/Radius
219 NominalRPM = cnf_elem("nominalrpm", estimate, yell);
221 MinRPM = cnf_elem("minrpm", 1.0, silent);
222 MinRPM = Constrain(1.0, MinRPM, NominalRPM-1.0);
224 estimate = (xmain) ? 0.12 : 0.07; // guess solidity
225 estimate = estimate * M_PI*Radius/BladeNum;
226 BladeChord = cnf_elem("chord", estimate, "FT", yell);
228 LiftCurveSlope = cnf_elem("liftcurveslope", 6.0, yell); // "1/RAD"
230 estimate = sqr(BladeChord) * sqr(Radius) * 0.57;
231 BladeFlappingMoment = cnf_elem("flappingmoment", estimate, "SLUG*FT2", yell);
232 BladeFlappingMoment = Constrain(0.1, BladeFlappingMoment, 1e9);
234 BladeTwist = cnf_elem("twist", -0.17, "RAD", yell);
236 estimate = sqr(BladeChord) * BladeChord * 15.66; // might be really wrong!
237 BladeMassMoment = cnf_elem("massmoment", estimate, yell); // slug-ft
238 BladeMassMoment = Constrain(0.1, BladeMassMoment, 1e9);
240 TipLossB = cnf_elem("tiplossfactor", 0.98, silent);
242 estimate = 1.1 * BladeFlappingMoment * BladeNum;
243 PolarMoment = cnf_elem("polarmoment", estimate, "SLUG*FT2", silent);
244 PolarMoment = Constrain(0.1, PolarMoment, 1e9);
246 InflowLag = cnf_elem("inflowlag", 0.2, silent); // fixme, depends on size
248 estimate = (xmain) ? 0.0 : -0.06;
249 ShaftTilt = cnf_elem("shafttilt", estimate, "RAD", silent);
251 // ignore differences between teeter/hingeless/fully-articulated constructions
252 estimate = 0.05 * Radius;
253 HingeOffset = cnf_elem("hingeoffset", estimate, "FT", (xmain) ? silent : yell);
255 CantAngleD3 = cnf_elem("cantangle", 0.0, "RAD", silent);
257 // derived parameters
259 // precalc often used powers
260 R[0]=1.0; R[1]=Radius; R[2]=R[1]*R[1]; R[3]=R[2]*R[1]; R[4]=R[3]*R[1];
261 B[0]=1.0; B[1]=TipLossB; B[2]=B[1]*B[1]; B[3]=B[2]*B[1]; B[4]=B[3]*B[1]; B[5]=B[4]*B[1];
263 LockNumberByRho = LiftCurveSlope * BladeChord * R[4] / BladeFlappingMoment;
264 solidity = BladeNum * BladeChord / (M_PI * Radius);
266 // use simple orientations at the moment
267 if (flags & eTail) { // axis parallel to Y_body
268 theta_shaft = 0.0; // no tilt
269 phi_shaft = 0.5*M_PI;
271 // opposite direction if main rotor is spinning CW
272 if (xmain && (xmain->flags & eRotCW) ) {
273 phi_shaft = -phi_shaft;
275 } else { // more or less upright
276 theta_shaft = ShaftTilt;
280 // setup Shaft-Body transforms, see /SH79/ eqn(17,18)
281 double st = sin(theta_shaft);
282 double ct = cos(theta_shaft);
283 double sp = sin(phi_shaft);
284 double cp = cos(phi_shaft);
286 ShaftToBody.InitMatrix( ct, st*sp, st*cp,
290 BodyToShaft = ShaftToBody.Inverse();
293 nu = 0.001; // help the flow solver by providing some moving molecules
298 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
300 // calculate control-axes components of total airspeed at the hub.
301 // sets rotor orientation angle (beta) as side effect. /SH79/ eqn(19-22)
303 FGColumnVector3 FGRotor::rotor::hub_vel_body2ca( const FGColumnVector3 &uvw,
304 const FGColumnVector3 &pqr,
305 double a_ic, double b_ic)
308 FGColumnVector3 v_r, v_shaft, v_w;
309 FGColumnVector3 pos(RelDistance_xhub,0.0,RelHeight_zhub);
312 v_shaft = BodyToShaft * v_r;
314 beta_orient = atan2(v_shaft(eV),v_shaft(eU));
316 v_w(eU) = v_shaft(eU)*cos(beta_orient) + v_shaft(eV)*sin(beta_orient);
318 v_w(eW) = v_shaft(eW) - b_ic*v_shaft(eU) - a_ic*v_shaft(eV);
323 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
325 // express fuselage angular velocity in control axes /SH79/ eqn(30,31)
327 FGColumnVector3 FGRotor::rotor::fus_angvel_body2ca( const FGColumnVector3 &pqr)
329 FGColumnVector3 av_s_fus, av_w_fus;
331 av_s_fus = BodyToShaft * pqr;
333 av_w_fus(eP)= av_s_fus(eP)*cos(beta_orient) + av_s_fus(eQ)*sin(beta_orient);
334 av_w_fus(eQ)= - av_s_fus(eP)*sin(beta_orient) + av_s_fus(eQ)*cos(beta_orient);
335 av_w_fus(eR)= av_s_fus(eR);
340 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
342 // problem function passed to rk solver
344 double FGRotor::rotor::dnuFunction::pFunc(double x, double nu) {
346 d_nu = k_sat * (ct_lambda * (k_wor - nu) + k_theta) /
347 (2.0 * sqrt( mu2 + sqr(k_wor - nu)) );
348 d_nu = d_nu * k_flowscale - nu;
352 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
354 // merge params to keep the equation short
355 void FGRotor::rotor::dnuFunction::update_params(rotor *r, double ct_t01, double fs, double w) {
356 k_sat = 0.5* r->solidity * r->LiftCurveSlope;
357 ct_lambda = 1.0/2.0*r->B[2] + 1.0/4.0 * r->mu*r->mu;
358 k_wor = w/(r->Omega*r->Radius);
365 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
367 // Calculate rotor thrust and inflow-ratio (lambda), this is achieved by
368 // approximating a solution for the differential equation:
370 // dnu/dt = 1/tau ( Ct / (2*sqrt(mu^2+lambda^2)) - nu ) , /SH79/ eqn(26).
372 // Propper calculation of the inflow-ratio (lambda) is vital for the
373 // following calculations. Simple implementations (i.e. Newton-Raphson w/o
374 // checking) tend to oscillate or overshoot in the low speed region,
375 // therefore a more expensive solver is used.
377 // The flow_scale parameter is used to approximate a reduction of inflow
378 // if the helicopter is close to the ground, yielding to higher thrust,
379 // see /TA77/ eqn(10a). Doing the ground effect calculation here seems
380 // more favorable then to code it in the fdm_config.
382 void FGRotor::rotor::calc_flow_and_thrust(
383 double dt, double rho, double theta_0,
384 double Uw, double Ww, double flow_scale)
387 double ct_over_sigma = 0.0;
388 double ct_l, ct_t0, ct_t1;
391 mu = Uw/(Omega*Radius); // /SH79/ eqn(24)
393 ct_t0 = (1.0/3.0*B[3] + 1.0/2.0 * TipLossB*mu*mu - 4.0/(9.0*M_PI) * mu*mu*mu )*theta_0;
394 ct_t1 = (1.0/4.0*B[4] + 1.0/4.0 * B[2]*mu*mu)*BladeTwist;
396 // merge params together
397 flowEquation.update_params(this, ct_t0+ct_t1, flow_scale, Ww);
399 nu_ret = rk.evolve(nu, &flowEquation);
401 if (rk.getStatus() != FGRungeKutta::eNoError) { // never observed so far
402 cerr << "# IEHHHH [" << flags << "]: Solver Error - resetting!" << endl;
404 nu_ret = nu; // use old value and keep fingers crossed.
407 // keep an eye on the solver, but be quiet after a hundred messages
408 if (reports < 100 && rk.getIterations()>6) {
409 cerr << "# LOOK [" << flags << "]: Solver took "
410 << rk.getIterations() << " rounds." << endl;
413 cerr << "# stopped babbling after 100 notifications." << endl;
417 // now from nu to lambda, Ct, and Thrust
420 lambda = Ww/(Omega*Radius) - nu; // /SH79/ eqn(25)
422 ct_l = (1.0/2.0*B[2] + 1.0/4.0 * mu*mu)*lambda;
423 ct_over_sigma = (LiftCurveSlope/2.0)*(ct_l + ct_t0 + ct_t1); // /SH79/ eqn(27)
425 Thrust = BladeNum*BladeChord*Radius*rho*sqr(Omega*Radius) * ct_over_sigma;
427 Ct = ct_over_sigma * solidity;
428 v_induced = nu * (Omega*Radius);
430 if (dump_req && (flags & eMain) ) {
431 printf("# mu %f : nu %f lambda %f vi %f\n", mu, nu, lambda, v_induced);
436 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
438 // this is the most arcane part in the calculation chain the
439 // constants used should be reverted to more general parameters.
440 // otoh: it works also for smaller rotors, sigh!
441 // See /SH79/ eqn(36), and /BA41/ for a richer set of equations.
443 void FGRotor::rotor::calc_torque(double rho, double theta_0)
446 double cq_s_m[5], cq_over_sigma;
447 double l,m,t075; // shortcuts
449 t075 = theta_0 + 0.75 * BladeTwist;
454 cq_s_m[0] = 0.00109 - 0.0036*l - 0.0027*t075 - 1.10*sqr(l) - 0.545*l*t075 + 0.122*sqr(t075);
455 cq_s_m[2] = ( 0.00109 - 0.0027*t075 - 3.13*sqr(l) - 6.35*l*t075 - 1.93*sqr(t075) ) * sqr(m);
456 cq_s_m[3] = - 0.133*l*t075 * sqr(m)*m;
457 cq_s_m[4] = ( - 0.976*sqr(l) - 6.38*l*t075 - 5.26*sqr(t075) ) * sqr(m)*sqr(m);
459 cq_over_sigma = cq_s_m[0] + cq_s_m[2] + cq_s_m[3] + cq_s_m[4];
460 // guess an a (LiftCurveSlope) is included in eqn above, so check if there is a large
461 // influence when a_'other-model'/ a_'ch54' diverts from 1.0.
463 Qa0 = BladeNum * BladeChord * R[2] * rho * sqr(Omega*Radius);
465 // TODO: figure out how to handle negative cq_over_sigma/torque
467 Torque = Qa0 * cq_over_sigma;
472 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
474 // The coning angle doesn't apply for teetering rotors, but calculating
475 // doesn't hurt. /SH79/ eqn(29)
477 void FGRotor::rotor::calc_coning_angle(double rho, double theta_0)
479 double lock_gamma = LockNumberByRho * rho;
481 double a0_l = (1.0/6.0 * B[3] + 0.04 * mu*mu*mu) * lambda;
482 double a0_t0 = (1.0/8.0 * B[4] + 1.0/8.0 * B[2]*mu*mu) * theta_0;
483 double a0_t1 = (1.0/10.0 * B[5] + 1.0/12.0 * B[3]*mu*mu) * BladeTwist;
484 a0 = lock_gamma * ( a0_l + a0_t0 + a0_t1);
488 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
490 // Flapping angles relative to control axes /SH79/ eqn(32)
492 void FGRotor::rotor::calc_flapping_angles( double rho, double theta_0,
493 const FGColumnVector3 &pqr_fus_w)
495 double lock_gamma = LockNumberByRho * rho;
497 double mu2_2B2 = sqr(mu)/(2.0*B[2]);
498 double t075 = theta_0 + 0.75 * BladeTwist; // common approximation for rectangular blades
500 a_1 = 1.0/(1.0 - mu2_2B2) * (
501 (2.0*lambda + (8.0/3.0)*t075)*mu
502 + pqr_fus_w(eP)/Omega
503 - 16.0 * pqr_fus_w(eQ)/(B[4]*lock_gamma*Omega)
506 b_1 = 1.0/(1.0 + mu2_2B2) * (
508 - pqr_fus_w(eQ)/Omega
509 - 16.0 * pqr_fus_w(eP)/(B[4]*lock_gamma*Omega)
512 // used in force calc
513 a_dw = 1.0/(1.0 - mu2_2B2) * (
514 (2.0*lambda + (8.0/3.0)*t075)*mu
515 - 24.0 * pqr_fus_w(eQ)/(B[4]*lock_gamma*Omega)
516 * ( 1.0 - ( 0.29 * t075 / (Ct/solidity) ) )
522 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
526 void FGRotor::rotor::calc_drag_and_side_forces(double rho, double theta_0)
528 double cy_over_sigma ;
529 double t075 = theta_0 + 0.75 * BladeTwist;
531 H_drag = Thrust * a_dw;
534 0.75*b_1*lambda - 1.5*a0*mu*lambda + 0.25*a_1*b_1*mu
535 - a0*a_1*sqr(mu) + (1.0/6.0)*a0*a_1
536 - (0.75*mu*a0 - (1.0/3.0)*b_1 - 0.5*sqr(mu)*b_1)*t075
538 cy_over_sigma *= LiftCurveSlope/2.0;
540 J_side = BladeNum * BladeChord * Radius * rho * sqr(Omega*Radius) * cy_over_sigma;
545 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
547 // transform rotor forces from control axes to shaft axes, and express
548 // in body axes /SH79/ eqn(40,41)
550 FGColumnVector3 FGRotor::rotor::body_forces(double a_ic, double b_ic)
553 - H_drag*cos(beta_orient) - J_side*sin(beta_orient) + Thrust*b_ic,
554 - H_drag*sin(beta_orient) + J_side*cos(beta_orient) + Thrust*a_ic,
557 if (dump_req && (flags & eMain) ) {
558 printf("# abß: % f % f % f\n", a_ic, b_ic, beta_orient );
559 printf("# HJT: % .2f % .2f % .2f\n", H_drag, J_side, Thrust );
560 printf("# F_s: % .2f % .2f % .2f\n", F_s(1), F_s(2), F_s(3) );
562 F_h = ShaftToBody * F_s;
563 printf("# F_h: % .2f % .2f % .2f\n", F_h(1), F_h(2), F_h(3) );
566 return ShaftToBody * F_s;
569 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
571 // rotational sense is handled here
572 // still a todo: how to get propper values for 'BladeMassMoment'
573 // here might be a good place to tweak hovering stability, check /AM50/.
575 FGColumnVector3 FGRotor::rotor::body_moments(FGColumnVector3 F_h, double a_ic, double b_ic)
577 FGColumnVector3 M_s, M_hub, M_h;
579 FGColumnVector3 h_pos(RelDistance_xhub, 0.0, RelHeight_zhub);
581 // vermutlich ein biege moment, bzw.widerstands moment ~ d^3
582 double M_w_tilde = 0.0 ;
585 M_w_tilde = BladeMassMoment;
587 // cyclic flapping relative to shaft axes /SH79/ eqn(43)
588 a1s = a_1*cos(beta_orient) + b_1*sin(beta_orient) - b_ic;
589 b1s = b_1*cos(beta_orient) - a_1*sin(beta_orient) + a_ic;
591 // mind this: no HingeOffset, no additional pitch/roll moments
592 mf = 0.5 * (HingeOffset+HingeOffset_hover) * BladeNum * Omega*Omega * M_w_tilde;
597 if (flags & eRotCW) {
601 if (flags & eCoaxial) {
605 M_hub = ShaftToBody * M_s;
607 M_h = M_hub + (h_pos * F_h);
613 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
617 FGRotor::FGRotor(FGFDMExec *exec, Element* rotor_element, int num)
618 : FGThruster(exec, rotor_element, num)
621 FGColumnVector3 location, orientation;
622 Element *thruster_element;
624 PropertyManager = fdmex->GetPropertyManager();
625 dt = fdmex->GetDeltaT();
629 rho = 0.002356; // just a sane value
633 tailRotorPresent = false;
635 effective_tail_col = 0.001; // just a sane value
637 prop_inflow_ratio_lambda = 0.0;
638 prop_advance_ratio_mu = 0.0;
639 prop_inflow_ratio_induced_nu = 0.0;
640 prop_mr_torque = 0.0;
641 prop_thrust_coefficient = 0.0;
642 prop_coning_angle = 0.0;
644 prop_theta_downwash = prop_phi_downwash = 0.0;
646 hover_threshold = 0.0;
658 SetTransformType(FGForce::tCustom);
660 // get data from parent and 'mount' the rotor system
662 thruster_element = rotor_element->GetParent()->FindElement("sense");
663 if (thruster_element) {
664 Sense = thruster_element->GetDataAsNumber() >= 0.0 ? 1.0: -1.0;
667 thruster_element = rotor_element->GetParent()->FindElement("location");
668 if (thruster_element) location = thruster_element->FindElementTripletConvertTo("IN");
669 else cerr << "No thruster location found." << endl;
671 thruster_element = rotor_element->GetParent()->FindElement("orient");
672 if (thruster_element) orientation = thruster_element->FindElementTripletConvertTo("RAD");
673 else cerr << "No thruster orientation found." << endl;
675 SetLocation(location);
676 SetAnglesToBody(orientation);
678 // get main rotor parameters
679 mr.parent = rotor_element;
684 a_val = rotor_element->GetAttributeValue("variant");
685 if ( a_val == "coaxial" ) {
687 cerr << "# found 'coaxial' variant" << endl;
698 // get tail rotor parameters
699 tr.parent=rotor_element->FindElement("tailrotor");
701 tailRotorPresent = true;
703 tailRotorPresent = false;
704 cerr << "# No tailrotor found, assuming a single rotor." << endl;
707 if (tailRotorPresent) {
712 tr.configure(flags, &mr);
714 tr.RpmRatio = tr.NominalRPM/mr.NominalRPM; // 'connect'
717 /* remaining parameters */
720 double c_ground_effect = 0.0; // uh1 ~ 0.28 , larger values increase the effect
721 ground_effect_exp = 0.0;
722 ground_effect_shift = 0.0;
724 if (rotor_element->FindElement("cgroundeffect"))
725 c_ground_effect = rotor_element->FindElementValueAsNumber("cgroundeffect");
727 if (rotor_element->FindElement("groundeffectshift"))
728 ground_effect_shift = rotor_element->FindElementValueAsNumberConvertTo("groundeffectshift","FT");
730 // prepare calculations, see /TA77/
731 if (c_ground_effect > 1e-5) {
732 ground_effect_exp = 1.0 / ( 2.0*mr.Radius * c_ground_effect );
734 ground_effect_exp = 0.0; // disable
737 // smooth out jumps in hagl reported, otherwise the ground effect
738 // calculation would cause jumps too. 1Hz seems sufficient.
739 damp_hagl = Filter(1.0,dt);
742 // misc, experimental
743 if (rotor_element->FindElement("hoverthreshold"))
744 hover_threshold = rotor_element->FindElementValueAsNumberConvertTo("hoverthreshold", "FT/SEC");
746 if (rotor_element->FindElement("hoverscale"))
747 hover_scale = rotor_element->FindElementValueAsNumber("hoverscale");
749 // enable import-export
753 prop_rotorbrake->setDoubleValue(0.0);
754 prop_freewheel_factor->setDoubleValue(1.0);
760 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
767 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
769 // mea-culpa - the connection to the engine might be wrong, but the calling
770 // convention appears to be 'variable' too.
772 // return Thruster->Calculate((Eng_HP * hptoftlbssec)-Thruster->GetPowerRequired());
774 // Thrust = Thruster->Calculate(Thrust); // allow thruster to modify thrust (i.e. reversing)
776 // Here 'Calculate' takes thrust and estimates the power provided.
778 double FGRotor::Calculate(double PowerAvailable)
781 double A_IC; // lateral (roll) control in radians
782 double B_IC; // longitudinal (pitch) control in radians
783 double theta_col; // main rotor collective pitch in radians
784 double tail_col; // tail rotor collective in radians
787 double h_agl_ft = 0.0;
790 FGColumnVector3 UVW_h;
791 FGColumnVector3 PQR_h;
793 /* total vehicle velocity including wind effects in feet per second. */
794 Vt = fdmex->GetAuxiliary()->GetVt();
796 dt = fdmex->GetDeltaT(); // might be variable ?
798 dump_req = prop_DumpFlag;
801 // fetch often used values
802 rho = fdmex->GetAtmosphere()->GetDensity(); // slugs/ft^3.
804 UVW_h = fdmex->GetAuxiliary()->GetAeroUVW();
805 PQR_h = fdmex->GetAuxiliary()->GetAeroPQR();
807 // handle present RPM now, calc omega values.
809 if (RPM < mr.MinRPM) { // kludge, otherwise calculations go bananas
814 mr.Omega = (RPM/60.0)*2.0*M_PI;
816 if (tailRotorPresent) {
817 tr.ActualRPM = RPM*tr.RpmRatio;
818 tr.Omega = (RPM*tr.RpmRatio/60.0)*2.0*M_PI;
821 // read control inputs
823 A_IC = prop_lateral_ctrl->getDoubleValue();
824 B_IC = prop_longitudinal_ctrl->getDoubleValue();
825 theta_col = prop_collective_ctrl->getDoubleValue();
827 if (tailRotorPresent) {
828 tail_col = prop_antitorque_ctrl->getDoubleValue();
832 FGColumnVector3 vHub_ca = mr.hub_vel_body2ca(UVW_h,PQR_h,A_IC,B_IC);
833 FGColumnVector3 avFus_ca = mr.fus_angvel_body2ca(PQR_h);
836 h_agl_ft = fdmex->GetPropagate()->GetDistanceAGL();
838 double filtered_hagl;
839 filtered_hagl = damp_hagl.execute( h_agl_ft + ground_effect_shift );
841 // gnuplot> plot [-1:50] 1 - exp((-x/44)/.28)
842 double ge_factor = 1.0;
843 if (ground_effect_exp > 1e-5) {
844 ge_factor -= exp(-filtered_hagl*ground_effect_exp);
847 if (ge_factor<0.5) ge_factor=0.5;
850 printf("# GE h: %.3f (%.3f) f: %f\n", filtered_hagl, h_agl_ft + ground_effect_shift, ge_factor);
854 // EXPERIMENTAL: modify rotor for hover, see rotor::body_moments for the consequences
855 if (hover_threshold > 1e-5 && Vt < hover_threshold) {
856 double scale = 1.0 - Vt/hover_threshold;
857 mr.HingeOffset_hover = scale*hover_scale*mr.Radius;
859 mr.HingeOffset_hover = 0.0;
862 // all set, start calculations
866 mr.calc_flow_and_thrust(dt, rho, theta_col, vHub_ca(eU), vHub_ca(eW), ge_factor);
868 prop_inflow_ratio_lambda = mr.lambda;
869 prop_advance_ratio_mu = mr.mu;
870 prop_inflow_ratio_induced_nu = mr.nu;
871 prop_thrust_coefficient = mr.Ct;
873 mr.calc_coning_angle(rho, theta_col);
874 prop_coning_angle = mr.a0;
876 mr.calc_torque(rho, theta_col);
877 prop_mr_torque = mr.Torque;
879 mr.calc_flapping_angles(rho, theta_col, avFus_ca);
880 mr.calc_drag_and_side_forces(rho, theta_col);
882 FGColumnVector3 F_h_mr = mr.body_forces(A_IC,B_IC);
883 FGColumnVector3 M_h_mr = mr.body_moments(F_h_mr, A_IC, B_IC);
885 // export downwash angles
886 // theta: positive for downwash moving from +z_h towards +x_h
887 // phi: positive for downwash moving from +z_h towards -y_h
889 prop_theta_downwash = atan2( - UVW_h(eU), mr.v_induced - UVW_h(eW));
890 prop_phi_downwash = atan2( UVW_h(eV), mr.v_induced - UVW_h(eW));
892 mr.force(eX) = F_h_mr(1);
893 mr.force(eY) = F_h_mr(2);
894 mr.force(eZ) = F_h_mr(3);
896 mr.moment(eL) = M_h_mr(1);
897 mr.moment(eM) = M_h_mr(2);
898 mr.moment(eN) = M_h_mr(3);
902 FGColumnVector3 F_h_tr(0.0, 0.0, 0.0);
903 FGColumnVector3 M_h_tr(0.0, 0.0, 0.0);
905 if (tailRotorPresent) {
906 FGColumnVector3 vHub_ca_tr = tr.hub_vel_body2ca(UVW_h,PQR_h);
907 FGColumnVector3 avFus_ca_tr = tr.fus_angvel_body2ca(PQR_h);
909 tr.calc_flow_and_thrust(dt, rho, tail_col, vHub_ca_tr(eU), vHub_ca_tr(eW));
910 tr.calc_coning_angle(rho, tail_col);
912 // test code for cantered tail rotor, see if it has a notable effect. /SH79/ eqn(47)
913 if (fabs(tr.CantAngleD3)>1e-5) {
914 double tan_d3 = tan(tr.CantAngleD3);
916 double ca_dt = dt/12.0;
917 for (int i = 0; i<12; i++) {
918 d_t0t = 1/0.1*(tail_col - tr.a0 * tan_d3 - effective_tail_col);
919 effective_tail_col += d_t0t*ca_dt;
922 effective_tail_col = tail_col;
925 tr.calc_torque(rho, effective_tail_col);
926 tr.calc_flapping_angles(rho, effective_tail_col, avFus_ca_tr);
927 tr.calc_drag_and_side_forces(rho, effective_tail_col);
929 F_h_tr = tr.body_forces();
930 M_h_tr = tr.body_moments(F_h_tr);
933 tr.force(eX) = F_h_tr(1) ;
934 tr.force(eY) = F_h_tr(2) ;
935 tr.force(eZ) = F_h_tr(3) ;
936 tr.moment(eL) = M_h_tr(1) ;
937 tr.moment(eM) = M_h_tr(2) ;
938 tr.moment(eN) = M_h_tr(3) ;
942 check negative mr.Torque conditions
943 freewheel factor: assure [0..1] just multiply with available power
944 rotorbrake: just steal from available power
948 // calculate new RPM, assuming a stiff connection between engine and rotor.
950 double engine_hp = PowerAvailable/2.24; // 'undo' force via the estimation factor used in aeromatic
951 double engine_torque = 550.0*engine_hp/mr.Omega;
952 double Omega_dot = (engine_torque - mr.Torque) / mr.PolarMoment;
954 RPM += ( Omega_dot * dt )/(2.0*M_PI) * 60.0;
957 printf("# SENSE : % d % d\n", mr.flags & eRotCW ? -1 : 1, tr.flags & eRotCW ? -1 : 1);
958 printf("# vi : % f % f\n", mr.v_induced, tr.v_induced);
959 printf("# a0 a1 b1 : % f % f % f\n", mr.a0, mr.a_1, mr.b_1 );
960 printf("# m forces : % f % f % f\n", mr.force(eX), mr.force(eY), mr.force(eZ) );
961 printf("# m moments : % f % f % f\n", mr.moment(eL), mr.moment(eM), mr.moment(eN) );
962 printf("# t forces : % f % f % f\n", tr.force(eX), tr.force(eY), tr.force(eZ) );
963 printf("# t moments : % f % f % f\n", tr.moment(eL), tr.moment(eM), tr.moment(eN) );
966 // finally set vFn & vMn
967 vFn = mr.force + tr.force;
968 vMn = mr.moment + tr.moment;
973 // return unmodified thrust to the turbine.
974 // :TK: As far as I can see the return value is unused.
975 return PowerAvailable;
979 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
981 // FGThruster does return 0.0 (the implicit direct thruster)
982 // piston CALL: return Thruster->Calculate((Eng_HP * hptoftlbssec)-Thruster->GetPowerRequired());
984 double FGRotor::GetPowerRequired(void)
987 return PowerRequired;
990 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
992 bool FGRotor::bind(void) {
994 string property_name, base_property_name;
995 base_property_name = CreateIndexedPropertyName("propulsion/engine", EngineNum);
997 PropertyManager->Tie( base_property_name + "/rotor-rpm", this, &FGRotor::GetRPM );
998 PropertyManager->Tie( base_property_name + "/thrust-mr-lbs", &mr.Thrust );
999 PropertyManager->Tie( base_property_name + "/vi-mr-fps", &mr.v_induced );
1000 PropertyManager->Tie( base_property_name + "/a0-mr-rad", &mr.a0 );
1001 PropertyManager->Tie( base_property_name + "/a1-mr-rad", &mr.a1s ); // s means shaft axes
1002 PropertyManager->Tie( base_property_name + "/b1-mr-rad", &mr.b1s );
1003 PropertyManager->Tie( base_property_name + "/thrust-tr-lbs", &tr.Thrust );
1004 PropertyManager->Tie( base_property_name + "/vi-tr-fps", &tr.v_induced );
1007 PropertyManager->Tie( base_property_name + "/inflow-ratio", &prop_inflow_ratio_lambda );
1009 PropertyManager->Tie( base_property_name + "/advance-ratio", &prop_advance_ratio_mu );
1011 PropertyManager->Tie( base_property_name + "/induced-inflow-ratio", &prop_inflow_ratio_induced_nu );
1013 PropertyManager->Tie( base_property_name + "/torque-mr-lbsft", &prop_mr_torque );
1014 PropertyManager->Tie( base_property_name + "/thrust-coefficient", &prop_thrust_coefficient );
1015 PropertyManager->Tie( base_property_name + "/main-rotor-rpm", &mr.ActualRPM );
1016 PropertyManager->Tie( base_property_name + "/tail-rotor-rpm", &tr.ActualRPM );
1018 // position of the downwash
1019 PropertyManager->Tie( base_property_name + "/theta-downwash-rad", &prop_theta_downwash );
1020 PropertyManager->Tie( base_property_name + "/phi-downwash-rad", &prop_phi_downwash );
1022 // nodes to use via get<xyz>Value
1023 prop_collective_ctrl = PropertyManager->GetNode(base_property_name + "/collective-ctrl-rad",true);
1024 prop_lateral_ctrl = PropertyManager->GetNode(base_property_name + "/lateral-ctrl-rad",true);
1025 prop_longitudinal_ctrl = PropertyManager->GetNode(base_property_name + "/longitudinal-ctrl-rad",true);
1026 prop_antitorque_ctrl = PropertyManager->GetNode(base_property_name + "/antitorque-ctrl-rad",true);
1028 prop_rotorbrake = PropertyManager->GetNode(base_property_name + "/rotorbrake-hp", true);
1029 prop_freewheel_factor = PropertyManager->GetNode(base_property_name + "/freewheel-factor", true);
1031 PropertyManager->Tie( base_property_name + "/dump-flag", &prop_DumpFlag );
1036 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
1038 string FGRotor::GetThrusterLabels(int id, string delimeter)
1041 std::ostringstream buf;
1043 buf << Name << " RPM (engine " << id << ")";
1049 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
1051 string FGRotor::GetThrusterValues(int id, string delimeter)
1053 std::ostringstream buf;
1060 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
1061 // The bitmasked value choices are as follows:
1062 // unset: In this case (the default) JSBSim would only print
1063 // out the normally expected messages, essentially echoing
1064 // the config files as they are read. If the environment
1065 // variable is not set, debug_lvl is set to 1 internally
1066 // 0: This requests JSBSim not to output any messages
1068 // 1: This value explicity requests the normal JSBSim
1070 // 2: This value asks for a message to be printed out when
1071 // a class is instantiated
1072 // 4: When this value is set, a message is displayed when a
1073 // FGModel object executes its Run() method
1074 // 8: When this value is set, various runtime state variables
1075 // are printed out periodically
1076 // 16: When set various parameters are sanity checked and
1077 // a message is printed out when they go out of bounds
1079 void FGRotor::Debug(int from)
1081 if (debug_lvl <= 0) return;
1083 if (debug_lvl & 1) { // Standard console startup message output
1084 if (from == 0) { // Constructor
1085 cout << "\n Rotor Name: " << Name << endl;
1088 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
1089 if (from == 0) cout << "Instantiated: FGRotor" << endl;
1090 if (from == 1) cout << "Destroyed: FGRotor" << endl;
1092 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
1094 if (debug_lvl & 8 ) { // Runtime state variables
1096 if (debug_lvl & 16) { // Sanity checking
1098 if (debug_lvl & 64) {
1099 if (from == 0) { // Constructor
1100 cout << IdSrc << endl;
1101 cout << IdHdr << endl;
1106 } // namespace JSBSim