47 #include "FGInitialCondition.h"
48 #include "models/FGInertial.h"
49 #include "models/FGAtmosphere.h"
50 #include "models/FGAccelerations.h"
51 #include "models/FGAuxiliary.h"
52 #include "input_output/FGXMLFileRead.h"
54 #include "FGFDMExec.h"
62 FGInitialCondition::FGInitialCondition(
FGFDMExec *FDMExec) : fdmex(FDMExec)
70 cout <<
"FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl;
86 double p0,
double q0,
double r0,
87 double alpha0,
double beta0,
88 double phi0,
double theta0,
double psi0,
89 double latRad0,
double lonRad0,
double altAGLFt0,
92 double calpha = cos(alpha0), cbeta = cos(beta0);
93 double salpha = sin(alpha0), sbeta = sin(beta0);
97 vPQR_body = {p0, q0, r0};
98 alpha = alpha0; beta = beta0;
102 fdmex->
GetInertial()->SetAltitudeAGL(position, altAGLFt0);
103 lastLatitudeSet = setgeoc;
104 lastAltitudeSet = setagl;
111 lastSpeedSet = setuvw;
113 Tw2b = { calpha*cbeta, -calpha*sbeta, -salpha,
115 salpha*cbeta, -salpha*sbeta, calpha };
136 vUVW_NED.InitMatrix();
137 vPQR_body.InitMatrix();
142 Tw2b = { 1., 0., 0., 0., 1., 0., 0., 0., 1. };
143 Tb2w = { 1., 0., 0., 0., 1., 0., 0., 0., 1. };
145 lastSpeedSet = setvt;
146 lastAltitudeSet = setasl;
147 lastLatitudeSet = setgeoc;
149 trimRequested = TrimMode::tNone;
156 const auto Atmosphere = fdmex->GetAtmosphere();
158 double rho = Atmosphere->GetDensity(altitudeASL);
159 SetVtrueFpsIC(ve*ktstofps*sqrt(FGAtmosphere::StdDaySLdensity/rho));
160 lastSpeedSet = setve;
167 const auto Atmosphere = fdmex->GetAtmosphere();
169 double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
171 lastSpeedSet = setmach;
178 const auto Atmosphere = fdmex->GetAtmosphere();
180 double pressure = Atmosphere->GetPressure(altitudeASL);
181 double mach = Auxiliary->MachFromVcalibrated(fabs(vcas)*ktstofps, pressure);
182 double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
185 lastSpeedSet = setvc;
192 void FGInitialCondition::calcAeroAngles(
const FGColumnVector3& _vt_NED)
196 double ua = _vt_BODY(eX);
197 double va = _vt_BODY(eY);
198 double wa = _vt_BODY(eZ);
199 double uwa = sqrt(ua*ua + wa*wa);
200 double calpha, cbeta;
201 double salpha, sbeta;
204 calpha = cbeta = 1.0;
205 salpha = sbeta = 0.0;
208 alpha = atan2( wa, ua );
217 beta = atan2( va, uwa );
229 Tw2b = { calpha*cbeta, -calpha*sbeta, -salpha,
231 salpha*cbeta, -salpha*sbeta, calpha };
248 _vt_NED = vUVW_NED + _vWIND_NED;
251 calcAeroAngles(_vt_NED);
253 lastSpeedSet = setvg;
270 _vt_NED *= vtrue / vt;
275 vUVW_NED = _vt_NED - _vWIND_NED;
277 calcAeroAngles(_vt_NED);
279 lastSpeedSet = setvt;
289 if (fabs(hdot) > vt) {
290 cerr <<
"The climb rate cannot be higher than the true speed." << endl;
297 double hdot0 = -_vt_NED(eW);
299 if (fabs(hdot0) < vt) {
300 double scale = sqrt((vt*vt-hdot*hdot)/(vt*vt-hdot0*hdot0));
301 _vt_NED(eU) *= scale;
302 _vt_NED(eV) *= scale;
305 vUVW_NED = _vt_NED - _WIND_NED;
308 calcThetaBeta(alpha, _vt_NED);
320 calcThetaBeta(alfa, _vt_NED);
328 void FGInitialCondition::calcThetaBeta(
double alfa,
const FGColumnVector3& _vt_NED)
331 double calpha = cos(alfa), salpha = sin(alfa);
342 -salpha, 0., calpha);
347 FGColumnVector3 u = y - DotProduct(y, n) * n;
348 FGColumnVector3 p = y * n;
350 if (DotProduct(p, v0) < 0) p *= -1.0;
353 u *= DotProduct(v0, y) / DotProduct(u, y);
363 if (DotProduct(v0, v0) < DotProduct(u, u)) {
364 cerr <<
"Cannot modify angle 'alpha' from " << alpha <<
" to " << alfa << endl;
368 FGColumnVector3 v1 = u + sqrt(DotProduct(v0, v0) - DotProduct(u, u))*p;
370 FGColumnVector3 v0xz(v0(eU), 0., v0(eW));
371 FGColumnVector3 v1xz(v1(eU), 0., v1(eW));
374 double sinTheta = (v1xz * v0xz)(eY);
375 vOrient(eTht) = asin(sinTheta);
377 orientation = FGQuaternion(vOrient);
379 const FGMatrix33& Tl2b = orientation.
GetT();
380 FGColumnVector3 v2 = Talpha * Tl2b * _vt_NED;
383 beta = atan2(v2(eV), v2(eU));
384 double cbeta=1.0, sbeta=0.0;
389 Tw2b = { calpha*cbeta, -calpha*sbeta, -salpha,
391 salpha*cbeta, -salpha*sbeta, calpha };
408 double calpha = cos(alpha), salpha = sin(alpha);
409 double cbeta = cos(beta), sbeta = sin(beta);
415 Tw2b = { calpha*cbeta, -calpha*sbeta, -salpha,
417 salpha*cbeta, -salpha*sbeta, calpha };
422 FGColumnVector3 v1xy(sqrt(v0xy(eX)*v0xy(eX)+v0xy(eY)*v0xy(eY)-vf(eY)*vf(eY)),vf(eY),0.);
426 if (vf(eX) < 0.) v0xy(eX) *= -1.0;
428 double sinPsi = (v1xy * v0xy)(eZ);
429 double cosPsi = DotProduct(v0xy, v1xy);
430 vOrient(ePsi) = atan2(sinPsi, cosPsi);
437 v2xz(eV) = vfxz(eV) = 0.0;
440 double sinTheta = (v2xz * vfxz)(eY);
441 vOrient(eTht) = -asin(sinTheta);
449 void FGInitialCondition::SetEulerAngleRadIC(
int idx,
double angle)
458 vOrient(idx) = angle;
461 if ((lastSpeedSet != setned) && (lastSpeedSet != setvg)) {
463 vUVW_NED = newTb2l * _vUVW_BODY;
464 _vt_NED = vUVW_NED + _vWIND_NED;
468 calcAeroAngles(_vt_NED);
476 void FGInitialCondition::SetBodyVelFpsIC(
int idx,
double vel)
478 const FGMatrix33& Tb2l = orientation.
GetTInv();
479 const FGMatrix33& Tl2b = orientation.
GetT();
480 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
481 FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED;
482 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
484 _vUVW_BODY(idx) = vel;
485 vUVW_NED = Tb2l * _vUVW_BODY;
486 _vt_NED = vUVW_NED + _vWIND_NED;
487 vt = _vt_NED.Magnitude();
489 calcAeroAngles(_vt_NED);
491 lastSpeedSet = setuvw;
499 void FGInitialCondition::SetNEDVelFpsIC(
int idx,
double vel)
501 const FGMatrix33& Tb2l = orientation.
GetTInv();
502 FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.);
503 FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED;
506 _vt_NED = vUVW_NED + _vWIND_NED;
507 vt = _vt_NED.Magnitude();
509 calcAeroAngles(_vt_NED);
511 lastSpeedSet = setned;
523 calcAeroAngles(_vt_NED);
539 _vWIND_NED -= DotProduct(_vWIND_NED, _vCROSS) * _vCROSS;
542 _vWIND_NED += (cross * ktstofps) * _vCROSS;
543 _vt_NED = vUVW_NED + _vWIND_NED;
546 calcAeroAngles(_vt_NED);
566 _vWIND_NED -= DotProduct(_vWIND_NED, _vHEAD) * _vHEAD;
569 _vWIND_NED += (head * ktstofps) * _vHEAD;
570 _vt_NED = vUVW_NED + _vWIND_NED;
574 calcAeroAngles(_vt_NED);
587 _vt_NED(eW) = vUVW_NED(eW) + wD * ktstofps;
590 calcAeroAngles(_vt_NED);
607 _vHEAD *= (mag*ktstofps) / windMag;
609 _vHEAD = {mag*ktstofps, 0., 0.};
611 _vWIND_NED(eU) = _vHEAD(eU);
612 _vWIND_NED(eV) = _vHEAD(eV);
613 _vt_NED = vUVW_NED + _vWIND_NED;
616 calcAeroAngles(_vt_NED);
629 double mag = _vWIND_NED.
Magnitude(eU, eV);
630 FGColumnVector3 _vHEAD(mag*cos(dir*degtorad), mag*sin(dir*degtorad), 0.);
632 _vWIND_NED(eU) = _vHEAD(eU);
633 _vWIND_NED(eV) = _vHEAD(eV);
634 _vt_NED = vUVW_NED + _vWIND_NED;
637 calcAeroAngles(_vt_NED);
647 if (lastAltitudeSet == setagl)
662 return fdmex->
GetInertial()->GetAltitudeAGL(position);
674 fdmex->
GetInertial()->GetContactPoint(position, contact, normal, v, w);
682 const auto Atmosphere = fdmex->GetAtmosphere();
684 double pressure = Atmosphere->GetPressure(altitudeASL);
685 double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
686 double rho = Atmosphere->GetDensity(altitudeASL);
688 double mach0 = vt / soundSpeed;
689 double vc0 = Auxiliary->VcalibratedFromMach(mach0, pressure);
690 double ve0 = vt * sqrt(rho/FGAtmosphere::StdDaySLdensity);
692 switch(lastLatitudeSet) {
694 fdmex->
GetInertial()->SetAltitudeAGL(position, agl);
700 double e2 = 1.0-b*b/(a*a);
707 double h = -2.0*max(a,b);
709 while ((fabs(n-prev_n) > 1E-15 || fabs(h-agl) > 1E-10) && iter < 10) {
710 geodLat = atan(tanlat/(1-n));
714 double sinGeodLat = sin(geodLat);
715 double N = a/sqrt(1-e2*sinGeodLat*sinGeodLat);
725 soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
726 rho = Atmosphere->GetDensity(altitudeASL);
727 pressure = Atmosphere->GetPressure(altitudeASL);
729 switch(lastSpeedSet) {
731 mach0 = Auxiliary->MachFromVcalibrated(vc0, pressure);
738 SetVtrueFpsIC(ve0 * sqrt(FGAtmosphere::StdDaySLdensity/rho));
744 lastAltitudeSet = setagl;
754 const auto Atmosphere = fdmex->GetAtmosphere();
756 double pressure = Atmosphere->GetPressure(altitudeASL);
757 double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
758 double rho = Atmosphere->GetDensity(altitudeASL);
760 double mach0 = vt / soundSpeed;
761 double vc0 = Auxiliary->VcalibratedFromMach(mach0, pressure);
762 double ve0 = vt * sqrt(rho/FGAtmosphere::StdDaySLdensity);
764 switch(lastLatitudeSet) {
771 double e2 = 1.0-b*b/(a*a);
773 double cosGeodLat = cos(geodLatitude);
774 double sinGeodLat = sin(geodLatitude);
775 double N = a/sqrt(1-e2*sinGeodLat*sinGeodLat);
776 double geodAlt = 0.0;
782 if (cosGeodLat > fabs(sinGeodLat)) {
783 double tanGeodLat = sinGeodLat/cosGeodLat;
784 double x0 = N*e2*cosGeodLat;
786 while (fabs(n-prev_n) > 1E-15 && iter < 10) {
787 double tanLat = (1-n)*tanGeodLat;
788 double cos2Lat = 1./(1.+tanLat*tanLat);
789 double slr = b/sqrt(1.-e2*cos2Lat);
790 double R = slr + alt;
796 geodAlt = x/cosGeodLat-N;
799 double cotanGeodLat = cosGeodLat/sinGeodLat;
800 double z0 = N*e2*sinGeodLat;
802 while (fabs(n-prev_n) > 1E-15 && iter < 10) {
803 double cotanLat = cotanGeodLat/(1-n);
804 double sin2Lat = 1./(1.+cotanLat*cotanLat);
805 double cos2Lat = 1.-sin2Lat;
806 double slr = b/sqrt(1.-e2*cos2Lat);
807 double R = slr + alt;
808 z = R*sign(cotanLat)*sqrt(sin2Lat);
813 geodAlt = z/sinGeodLat-N*(1-e2);
829 soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
830 rho = Atmosphere->GetDensity(altitudeASL);
831 pressure = Atmosphere->GetPressure(altitudeASL);
833 switch(lastSpeedSet) {
835 mach0 = Auxiliary->MachFromVcalibrated(vc0, pressure);
842 SetVtrueFpsIC(ve0 * sqrt(FGAtmosphere::StdDaySLdensity/rho));
848 lastAltitudeSet = setasl;
856 lastLatitudeSet = setgeod;
858 switch (lastAltitudeSet)
864 fdmex->
GetInertial()->SetAltitudeAGL(position, agl);
883 lastLatitudeSet = setgeoc;
885 switch(lastAltitudeSet) {
905 switch(lastAltitudeSet) {
925 return _vWIND_NED.
Magnitude(eU, eV) == 0.0 ? 0.0
926 : atan2(_vWIND_NED(eV), _vWIND_NED(eU))*radtodeg;
934 return _vt_NED - vUVW_NED;
950 double FGInitialCondition::GetBodyWindFpsIC(
int idx)
const
957 return _vWIND_BODY(idx);
964 const auto Atmosphere = fdmex->GetAtmosphere();
966 double pressure = Atmosphere->GetPressure(altitudeASL);
967 double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
968 double mach = vt / soundSpeed;
970 return fpstokts * Auxiliary->VcalibratedFromMach(mach, pressure);
977 const auto Atmosphere = fdmex->GetAtmosphere();
979 double rho = Atmosphere->GetDensity(altitudeASL);
980 return fpstokts * vt * sqrt(rho/FGAtmosphere::StdDaySLdensity);
987 const auto Atmosphere = fdmex->GetAtmosphere();
989 double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL);
990 return vt / soundSpeed;
995 double FGInitialCondition::GetBodyVelFpsIC(
int idx)
const
1000 return _vUVW_BODY(idx);
1007 SGPath init_file_name;
1008 if(useAircraftPath && rstfile.isRelative()) {
1011 init_file_name = rstfile;
1015 Element* document = XMLFileRead.LoadXMLDocument(init_file_name);
1020 s <<
"File: " << init_file_name <<
" could not be read.";
1021 cerr << s.str() << endl;
1025 if (document->
GetName() !=
"initialize") {
1027 s <<
"File: " << init_file_name <<
" is not a reset file.";
1028 cerr << s.str() << endl;
1032 bool result =
false;
1038 if (version >= 3.0) {
1039 const string s(
"Only initialization file formats 1 and 2 are currently supported");
1040 cerr << document->
ReadFrom() << endl << s << endl;
1042 }
else if (version >= 2.0) {
1043 result = Load_v2(document);
1044 }
else if (version >= 1.0) {
1045 result = Load_v1(document);
1049 result = Load_v1(document);
1054 while (running_elements) {
1056 enginesRunning |= engineNumber == -1 ? engineNumber : 1 << engineNumber;
1065 bool FGInitialCondition::LoadLatitude(
Element* position_el)
1072 if (fabs(latitude) > 0.5*M_PI) {
1074 if (unit_type.empty()) unit_type=
"RAD";
1076 cerr << latitude_el->
ReadFrom() <<
"The latitude value "
1078 <<
" is outside the range [";
1079 if (unit_type ==
"DEG")
1080 cerr <<
"-90 DEG ; +90 DEG]" << endl;
1082 cerr <<
"-PI/2 RAD; +PI/2 RAD]" << endl;
1089 if (lat_type ==
"geod" || lat_type ==
"geodetic") {
1091 lastLatitudeSet = setgeod;
1095 lastLatitudeSet = setgeoc;
1104 void FGInitialCondition::SetTrimRequest(std::string trim)
1106 std::string& trimOption = to_lower(trim);
1107 if (trimOption ==
"1")
1108 trimRequested = TrimMode::tGround;
1109 else if (trimOption ==
"longitudinal")
1110 trimRequested = TrimMode::tLongitudinal;
1111 else if (trimOption ==
"full")
1112 trimRequested = TrimMode::tFull;
1113 else if (trimOption ==
"ground")
1114 trimRequested = TrimMode::tGround;
1115 else if (trimOption ==
"pullup")
1116 trimRequested = TrimMode::tPullup;
1117 else if (trimOption ==
"custom")
1118 trimRequested = TrimMode::tCustom;
1119 else if (trimOption ==
"turn")
1120 trimRequested = TrimMode::tTurn;
1125 bool FGInitialCondition::Load_v1(Element* document)
1129 if (document->FindElement(
"longitude"))
1130 SetLongitudeRadIC(document->FindElementValueAsNumberConvertTo(
"longitude",
"RAD"));
1131 if (document->FindElement(
"elevation"))
1134 if (document->FindElement(
"altitude"))
1136 else if (document->FindElement(
"altitudeAGL"))
1137 SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo(
"altitudeAGL",
"FT"));
1138 else if (document->FindElement(
"altitudeMSL"))
1139 SetAltitudeASLFtIC(document->FindElementValueAsNumberConvertTo(
"altitudeMSL",
"FT"));
1141 result = LoadLatitude(document);
1143 FGColumnVector3 vOrient = orientation.
GetEuler();
1145 if (document->FindElement(
"phi"))
1146 vOrient(ePhi) = document->FindElementValueAsNumberConvertTo(
"phi",
"RAD");
1147 if (document->FindElement(
"theta"))
1148 vOrient(eTht) = document->FindElementValueAsNumberConvertTo(
"theta",
"RAD");
1149 if (document->FindElement(
"psi"))
1150 vOrient(ePsi) = document->FindElementValueAsNumberConvertTo(
"psi",
"RAD");
1152 orientation = FGQuaternion(vOrient);
1154 if (document->FindElement(
"ubody"))
1155 SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo(
"ubody",
"FT/SEC"));
1156 if (document->FindElement(
"vbody"))
1157 SetVBodyFpsIC(document->FindElementValueAsNumberConvertTo(
"vbody",
"FT/SEC"));
1158 if (document->FindElement(
"wbody"))
1159 SetWBodyFpsIC(document->FindElementValueAsNumberConvertTo(
"wbody",
"FT/SEC"));
1160 if (document->FindElement(
"vnorth"))
1161 SetVNorthFpsIC(document->FindElementValueAsNumberConvertTo(
"vnorth",
"FT/SEC"));
1162 if (document->FindElement(
"veast"))
1163 SetVEastFpsIC(document->FindElementValueAsNumberConvertTo(
"veast",
"FT/SEC"));
1164 if (document->FindElement(
"vdown"))
1165 SetVDownFpsIC(document->FindElementValueAsNumberConvertTo(
"vdown",
"FT/SEC"));
1166 if (document->FindElement(
"vc"))
1168 if (document->FindElement(
"vt"))
1169 SetVtrueKtsIC(document->FindElementValueAsNumberConvertTo(
"vt",
"KTS"));
1170 if (document->FindElement(
"mach"))
1171 SetMachIC(document->FindElementValueAsNumber(
"mach"));
1172 if (document->FindElement(
"gamma"))
1174 if (document->FindElement(
"roc"))
1175 SetClimbRateFpsIC(document->FindElementValueAsNumberConvertTo(
"roc",
"FT/SEC"));
1176 if (document->FindElement(
"vground"))
1177 SetVgroundKtsIC(document->FindElementValueAsNumberConvertTo(
"vground",
"KTS"));
1178 if (document->FindElement(
"alpha"))
1179 SetAlphaDegIC(document->FindElementValueAsNumberConvertTo(
"alpha",
"DEG"));
1180 if (document->FindElement(
"beta"))
1181 SetBetaDegIC(document->FindElementValueAsNumberConvertTo(
"beta",
"DEG"));
1182 if (document->FindElement(
"vwind"))
1183 SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo(
"vwind",
"KTS"));
1184 if (document->FindElement(
"winddir"))
1185 SetWindDirDegIC(document->FindElementValueAsNumberConvertTo(
"winddir",
"DEG"));
1186 if (document->FindElement(
"hwind"))
1187 SetHeadWindKtsIC(document->FindElementValueAsNumberConvertTo(
"hwind",
"KTS"));
1188 if (document->FindElement(
"xwind"))
1190 if (document->FindElement(
"targetNlf"))
1192 if (document->FindElement(
"trim"))
1193 SetTrimRequest(document->FindElementValue(
"trim"));
1195 vPQR_body.InitMatrix();
1202 bool FGInitialCondition::Load_v2(Element* document)
1204 FGColumnVector3 vOrient;
1208 if (document->FindElement(
"earth_position_angle"))
1209 epa = document->FindElementValueAsNumberConvertTo(
"earth_position_angle",
"RAD");
1210 if (document->FindElement(
"planet_position_angle"))
1211 epa = document->FindElementValueAsNumberConvertTo(
"planet_position_angle",
"RAD");
1214 FGMatrix33 Ti2ec(cos(epa), sin(epa), 0.0,
1215 -sin(epa), cos(epa), 0.0,
1217 FGMatrix33 Tec2i = Ti2ec.Transposed();
1219 if (document->FindElement(
"planet_rotation_rate")) {
1220 fdmex->
GetInertial()->SetOmegaPlanet(document->FindElementValueAsNumberConvertTo(
"planet_rotation_rate",
"RAD"));
1224 FGColumnVector3 vOmegaEarth = fdmex->
GetInertial()->GetOmegaPlanet();
1226 if (document->FindElement(
"elevation")) {
1227 fdmex->
GetInertial()->SetTerrainElevation(document->FindElementValueAsNumberConvertTo(
"elevation",
"FT"));
1236 Element* position_el = document->
FindElement(
"position");
1239 frame = to_lower(frame);
1240 if (frame ==
"eci") {
1241 position = Ti2ec * position_el->FindElementTripletConvertTo(
"FT");
1242 }
else if (frame ==
"ecef") {
1243 if (!position_el->FindElement(
"x") && !position_el->FindElement(
"y") && !position_el->FindElement(
"z")) {
1244 if (position_el->FindElement(
"longitude")) {
1245 SetLongitudeRadIC(position_el->FindElementValueAsNumberConvertTo(
"longitude",
"RAD"));
1247 if (position_el->FindElement(
"radius")) {
1248 position.
SetRadius(position_el->FindElementValueAsNumberConvertTo(
"radius",
"FT"));
1249 }
else if (position_el->FindElement(
"altitudeAGL")) {
1250 SetAltitudeAGLFtIC(position_el->FindElementValueAsNumberConvertTo(
"altitudeAGL",
"FT"));
1251 }
else if (position_el->FindElement(
"altitudeMSL")) {
1252 SetAltitudeASLFtIC(position_el->FindElementValueAsNumberConvertTo(
"altitudeMSL",
"FT"));
1254 cerr << endl <<
" No altitude or radius initial condition is given." << endl;
1259 result = LoadLatitude(position_el);
1262 position = position_el->FindElementTripletConvertTo(
"FT");
1265 cerr << endl <<
" Neither ECI nor ECEF frame is specified for initial position." << endl;
1269 cerr << endl <<
" Initial position not specified in this initialization file." << endl;
1298 Element* orientation_el = document->FindElement(
"orientation");
1299 if (orientation_el) {
1300 string frame = orientation_el->GetAttributeValue(
"frame");
1301 frame = to_lower(frame);
1302 vOrient = orientation_el->FindElementTripletConvertTo(
"RAD");
1303 if (frame ==
"eci") {
1315 FGQuaternion QuatI2Body = FGQuaternion(vOrient);
1316 QuatI2Body.Normalize();
1317 FGQuaternion QuatLocal2I = Tec2i * position.
GetTl2ec();
1318 QuatLocal2I.Normalize();
1319 orientation = QuatLocal2I * QuatI2Body;
1321 }
else if (frame ==
"ecef") {
1333 FGQuaternion QuatEC2Body(vOrient);
1334 QuatEC2Body.Normalize();
1335 FGQuaternion QuatLocal2EC = position.
GetTl2ec();
1336 QuatLocal2EC.Normalize();
1337 orientation = QuatLocal2EC * QuatEC2Body;
1339 }
else if (frame ==
"local") {
1341 orientation = FGQuaternion(vOrient);
1345 cerr << endl <<
fgred <<
" Orientation frame type: \"" << frame
1346 <<
"\" is not supported!" <<
reset << endl << endl;
1360 Element* velocity_el = document->FindElement(
"velocity");
1361 FGMatrix33 mTec2l = position.
GetTec2l();
1362 const FGMatrix33& Tb2l = orientation.
GetTInv();
1366 string frame = velocity_el->GetAttributeValue(
"frame");
1367 frame = to_lower(frame);
1368 FGColumnVector3 vInitVelocity = velocity_el->FindElementTripletConvertTo(
"FT/SEC");
1370 if (frame ==
"eci") {
1371 FGColumnVector3 omega_cross_r = vOmegaEarth * (Tec2i * position);
1372 vUVW_NED = mTec2l * (vInitVelocity - omega_cross_r);
1373 lastSpeedSet = setned;
1374 }
else if (frame ==
"ecef") {
1375 vUVW_NED = mTec2l * vInitVelocity;
1376 lastSpeedSet = setned;
1377 }
else if (frame ==
"local") {
1378 vUVW_NED = vInitVelocity;
1379 lastSpeedSet = setned;
1380 }
else if (frame ==
"body") {
1381 vUVW_NED = Tb2l * vInitVelocity;
1382 lastSpeedSet = setuvw;
1385 cerr << endl <<
fgred <<
" Velocity frame type: \"" << frame
1386 <<
"\" is not supported!" <<
reset << endl << endl;
1393 vUVW_NED.InitMatrix();
1397 vt = vUVW_NED.Magnitude();
1399 calcAeroAngles(vUVW_NED);
1407 Element* attrate_el = document->FindElement(
"attitude_rate");
1411 string frame = attrate_el->GetAttributeValue(
"frame");
1412 frame = to_lower(frame);
1413 const FGMatrix33& Tl2b = orientation.
GetT();
1414 FGColumnVector3 vAttRate = attrate_el->FindElementTripletConvertTo(
"RAD/SEC");
1416 if (frame ==
"eci") {
1417 FGMatrix33 Ti2l = position.
GetTec2l() * Ti2ec;
1418 vPQR_body = Tl2b * Ti2l * (vAttRate - vOmegaEarth);
1419 }
else if (frame ==
"ecef") {
1420 vPQR_body = Tl2b * position.
GetTec2l() * vAttRate;
1421 }
else if (frame ==
"local") {
1424 double radInv = 1.0 / position.
GetRadius();
1425 FGColumnVector3 vOmegaLocal = {radInv*vUVW_NED(eEast),
1426 -radInv*vUVW_NED(eNorth),
1427 -radInv*vUVW_NED(eEast)*tan(position.
GetLatitude())};
1428 vPQR_body = Tl2b * (vAttRate + vOmegaLocal);
1429 }
else if (frame ==
"body") {
1430 vPQR_body = vAttRate;
1431 }
else if (!frame.empty()) {
1433 cerr << endl <<
fgred <<
" Attitude rate frame type: \"" << frame
1434 <<
"\" is not supported!" <<
reset << endl << endl;
1437 }
else if (frame.empty()) {
1438 vPQR_body.InitMatrix();
1442 vPQR_body.InitMatrix();
1450 void FGInitialCondition::bind(FGPropertyManager* PropertyManager)
1452 PropertyManager->Tie(
"ic/vc-kts",
this,
1455 PropertyManager->Tie(
"ic/ve-kts",
this,
1458 PropertyManager->Tie(
"ic/vg-kts",
this,
1461 PropertyManager->Tie(
"ic/vt-kts",
this,
1464 PropertyManager->Tie(
"ic/mach",
this,
1467 PropertyManager->Tie(
"ic/roc-fpm",
this,
1470 PropertyManager->Tie(
"ic/gamma-deg",
this,
1473 PropertyManager->Tie(
"ic/alpha-deg",
this,
1476 PropertyManager->Tie(
"ic/beta-deg",
this,
1479 PropertyManager->Tie(
"ic/theta-deg",
this,
1482 PropertyManager->Tie(
"ic/phi-deg",
this,
1485 PropertyManager->Tie(
"ic/psi-true-deg",
this,
1488 PropertyManager->Tie(
"ic/lat-gc-deg",
this,
1491 PropertyManager->Tie(
"ic/long-gc-deg",
this,
1494 PropertyManager->Tie(
"ic/h-sl-ft",
this,
1497 PropertyManager->Tie(
"ic/h-agl-ft",
this,
1500 PropertyManager->Tie(
"ic/terrain-elevation-ft",
this,
1503 PropertyManager->Tie(
"ic/vg-fps",
this,
1506 PropertyManager->Tie(
"ic/vt-fps",
this,
1509 PropertyManager->Tie(
"ic/vw-bx-fps",
this,
1511 PropertyManager->Tie(
"ic/vw-by-fps",
this,
1513 PropertyManager->Tie(
"ic/vw-bz-fps",
this,
1515 PropertyManager->Tie(
"ic/vw-north-fps",
this,
1517 PropertyManager->Tie(
"ic/vw-east-fps",
this,
1519 PropertyManager->Tie(
"ic/vw-down-fps",
this,
1521 PropertyManager->Tie(
"ic/vw-mag-fps",
this,
1524 PropertyManager->Tie(
"ic/vw-dir-deg",
this,
1528 PropertyManager->Tie(
"ic/roc-fps",
this,
1531 PropertyManager->Tie(
"ic/u-fps",
this,
1534 PropertyManager->Tie(
"ic/v-fps",
this,
1537 PropertyManager->Tie(
"ic/w-fps",
this,
1540 PropertyManager->Tie(
"ic/vn-fps",
this,
1543 PropertyManager->Tie(
"ic/ve-fps",
this,
1546 PropertyManager->Tie(
"ic/vd-fps",
this,
1549 PropertyManager->Tie(
"ic/gamma-rad",
this,
1552 PropertyManager->Tie(
"ic/alpha-rad",
this,
1555 PropertyManager->Tie(
"ic/theta-rad",
this,
1558 PropertyManager->Tie(
"ic/beta-rad",
this,
1561 PropertyManager->Tie(
"ic/phi-rad",
this,
1564 PropertyManager->Tie(
"ic/psi-true-rad",
this,
1567 PropertyManager->Tie(
"ic/lat-gc-rad",
this,
1570 PropertyManager->Tie(
"ic/long-gc-rad",
this,
1573 PropertyManager->Tie(
"ic/p-rad_sec",
this,
1576 PropertyManager->Tie(
"ic/q-rad_sec",
this,
1579 PropertyManager->Tie(
"ic/r-rad_sec",
this,
1582 PropertyManager->Tie(
"ic/lat-geod-rad",
this,
1585 PropertyManager->Tie(
"ic/lat-geod-deg",
this,
1588 PropertyManager->Tie(
"ic/geod-alt-ft", &position,
1591 PropertyManager->Tie(
"ic/targetNlf",
this,
1615 void FGInitialCondition::Debug(
int from)
1617 if (debug_lvl <= 0)
return;
1619 if (debug_lvl & 1) {
1621 if (debug_lvl & 2 ) {
1622 if (from == 0) cout <<
"Instantiated: FGInitialCondition" << endl;
1623 if (from == 1) cout <<
"Destroyed: FGInitialCondition" << endl;
1625 if (debug_lvl & 4 ) {
1627 if (debug_lvl & 8 ) {
1629 if (debug_lvl & 16) {
1631 if (debug_lvl & 64) {
Element * FindElement(const std::string &el="")
Searches for a specified element.
double GetAttributeValueAsNumber(const std::string &key)
Retrieves an attribute value as a double precision real number.
std::string GetAttributeValue(const std::string &key)
Retrieves an attribute.
std::string ReadFrom(void) const
Return a string that contains a description of the location where the current XML element was read fr...
double FindElementValueAsNumberConvertTo(const std::string &el, const std::string &target_units)
Searches for the named element and converts and returns the data belonging to it.
Element * FindNextElement(const std::string &el="")
Searches for the next element as specified.
double GetDataAsNumber(void)
Converts the element data to a number.
bool HasAttribute(const std::string &key)
Determines if an element has the supplied attribute.
const std::string & GetName(void) const
Retrieves the element name.
This class implements a 3 element column vector.
double Magnitude(void) const
Length of the vector.
FGColumnVector3 & Normalize(void)
Normalize.
Encapsulates the JSBSim simulation executive.
const SGPath & GetFullAircraftPath(void)
Retrieves the full aircraft path name.
std::shared_ptr< FGAircraft > GetAircraft(void) const
Returns the FGAircraft pointer.
std::shared_ptr< FGPropagate > GetPropagate(void) const
Returns the FGPropagate pointer.
std::shared_ptr< FGInertial > GetInertial(void) const
Returns the FGInertial pointer.
std::shared_ptr< FGAuxiliary > GetAuxiliary(void) const
Returns the FGAuxiliary pointer.
std::shared_ptr< FGAccelerations > GetAccelerations(void) const
Returns the FGAccelerations pointer.
void SetPsiRadIC(double psi)
Sets the initial heading angle.
double GetPRadpsIC() const
Gets the initial body axis roll rate.
double GetVNorthFpsIC(void) const
Gets the initial local frame X (North) velocity.
void SetThetaDegIC(double theta)
Sets pitch angle initial condition in degrees.
void SetVequivalentKtsIC(double ve)
Set equivalent airspeed initial condition in knots.
double GetPhiRadIC(void) const
Gets the initial roll angle.
double GetVBodyFpsIC(void) const
Gets the initial body axis Y velocity.
double GetVEastFpsIC(void) const
Gets the initial local frame Y (East) velocity.
double GetLatitudeDegIC(void) const
Gets the initial latitude.
void SetWindDownKtsIC(double wD)
Sets the initial wind downward speed.
void ResetIC(double u0, double v0, double w0, double p0, double q0, double r0, double alpha0, double beta0, double phi0, double theta0, double psi0, double latitudeRad0, double longitudeRad0, double altitudeAGL0, double gamma0)
Resets the IC data structure to new values.
double GetVgroundFpsIC(void) const
Gets the initial ground velocity.
double GetLongitudeRadIC(void) const
Gets the initial longitude.
double GetFlightPathAngleDegIC(void) const
Gets the initial flight path angle.
double GetThetaDegIC(void) const
Gets the initial pitch angle.
double GetVgroundKtsIC(void) const
Gets the initial ground speed.
void SetUBodyFpsIC(double ubody)
Sets the initial body axis X velocity.
void SetWindMagKtsIC(double mag)
Sets the initial total wind speed.
double GetPhiDegIC(void) const
Gets the initial roll angle.
void InitializeIC(void)
Initialize the initial conditions to default values.
double GetAltitudeASLFtIC(void) const
Gets the initial altitude above sea level.
void SetClimbRateFpmIC(double roc)
Sets the climb rate initial condition in feet/minute.
double GetWindNFpsIC(void) const
Gets the initial wind velocity in local frame.
void SetFlightPathAngleRadIC(double gamma)
Sets the initial flight path angle.
void SetWBodyFpsIC(double wbody)
Sets the initial body axis Z velocity.
double GetTargetNlfIC(void) const
Gets the target normal load factor set from IC.
void SetMachIC(double mach)
Set mach initial condition.
void SetVtrueKtsIC(double vtrue)
Set true airspeed initial condition in knots.
void SetBetaRadIC(double beta)
Sets the initial sideslip angle.
double GetWindEFpsIC(void) const
Gets the initial wind velocity in local frame.
double GetTerrainElevationFtIC(void) const
Gets the initial terrain elevation.
void SetWindNEDFpsIC(double wN, double wE, double wD)
Sets the initial wind velocity.
void SetCrossWindKtsIC(double cross)
Sets the initial crosswind speed.
void SetVBodyFpsIC(double vbody)
Sets the initial body axis Y velocity.
void SetLongitudeRadIC(double lon)
Sets the initial longitude.
void SetAlphaDegIC(double a)
Sets angle of attack initial condition in degrees.
void SetPRadpsIC(double P)
Sets the initial body axis roll rate.
double GetVDownFpsIC(void) const
Gets the initial local frame Z (Down) velocity.
double GetVcalibratedKtsIC(void) const
Gets the initial calibrated airspeed.
double GetWindDirDegIC(void) const
Gets the initial wind direction.
void SetFlightPathAngleDegIC(double gamma)
Sets the flight path angle initial condition in degrees.
double GetWBodyFpsIC(void) const
Gets the initial body axis Z velocity.
double GetBetaDegIC(void) const
Gets the initial sideslip angle.
FGColumnVector3 GetWindNEDFpsIC(void) const
Gets the initial wind velocity in the NED local frame.
void SetVgroundKtsIC(double vg)
Set ground speed initial condition in knots.
void SetLongitudeDegIC(double lon)
Sets the initial longitude.
double GetClimbRateFpsIC(void) const
Gets the initial climb rate.
void SetGeodLatitudeRadIC(double glat)
Sets the initial geodetic latitude.
void SetAltitudeAGLFtIC(double agl)
Sets the initial Altitude above ground level.
~FGInitialCondition()
Destructor.
double GetWindMagFpsIC(void) const
Gets the initial total wind velocity in feet/sec.
double GetAltitudeAGLFtIC(void) const
Gets the initial altitude above ground level.
double GetMachIC(void) const
Gets the initial mach.
void SetThetaRadIC(double theta)
Sets the initial pitch angle.
double GetVtrueFpsIC(void) const
Gets the initial true velocity.
double GetGeodLatitudeDegIC(void) const
Gets the initial geodetic latitude.
void SetTerrainElevationFtIC(double elev)
Sets the initial terrain elevation.
double GetRRadpsIC() const
Gets the initial body axis yaw rate.
double GetAlphaRadIC(void) const
Gets the initial angle of attack.
double GetAlphaDegIC(void) const
Gets the initial angle of attack.
void SetClimbRateFpsIC(double roc)
Sets the initial climb rate.
void SetTargetNlfIC(double nlf)
Sets the target normal load factor.
void SetVtrueFpsIC(double vt)
Sets the initial true airspeed.
void SetPsiDegIC(double psi)
Sets the heading angle initial condition in degrees.
void SetAlphaRadIC(double alpha)
Sets the initial angle of attack.
double GetGeodLatitudeRadIC(void) const
Gets the initial geodetic latitude.
void SetHeadWindKtsIC(double head)
Sets the initial headwind velocity.
void SetRRadpsIC(double R)
Sets the initial body axis yaw rate.
void SetQRadpsIC(double Q)
Sets the initial body axis pitch rate.
double GetUBodyFpsIC(void) const
Gets the initial body axis X velocity.
double GetQRadpsIC() const
Gets the initial body axis pitch rate.
void SetVDownFpsIC(double vd)
Sets the initial local axis down velocity.
void SetLatitudeRadIC(double lat)
Sets the initial latitude.
void SetGeodLatitudeDegIC(double glat)
Sets the initial geodetic latitude.
void SetVcalibratedKtsIC(double vc)
Set calibrated airspeed initial condition in knots.
double GetVequivalentKtsIC(void) const
Gets the initial equivalent airspeed.
void SetPhiDegIC(double phi)
Sets the roll angle initial condition in degrees.
void SetBetaDegIC(double b)
Sets angle of sideslip initial condition in degrees.
double GetFlightPathAngleRadIC(void) const
Gets the initial flight path angle.
double GetPsiRadIC(void) const
Gets the initial heading angle.
bool Load(const SGPath &rstname, bool useAircraftPath=true)
Loads the initial conditions.
void SetWindDirDegIC(double dir)
Sets the initial wind direction.
double GetClimbRateFpmIC(void) const
Gets the initial climb rate.
double GetThetaRadIC(void) const
Gets the initial pitch angle.
void SetVNorthFpsIC(double vn)
Sets the initial local axis north velocity.
void SetPhiRadIC(double phi)
Sets the initial roll angle.
double GetWindUFpsIC(void) const
Gets the initial body axis X wind velocity.
void SetLatitudeDegIC(double lat)
Sets the initial latitude.
double GetWindVFpsIC(void) const
Gets the initial body axis Y wind velocity.
void SetAltitudeASLFtIC(double altitudeASL)
Sets the altitude above sea level initial condition in feet.
void SetWindMagFpsIC(double mag)
Sets the initial total wind speed.
double GetVtrueKtsIC(void) const
Gets the initial true velocity.
double GetPsiDegIC(void) const
Gets the initial heading angle.
double GetLatitudeRadIC(void) const
Gets the initial latitude.
double GetWindDFpsIC(void) const
Gets the initial wind velocity in local frame.
void SetVEastFpsIC(double ve)
Sets the initial local axis east velocity.
void SetVgroundFpsIC(double vg)
Sets the initial ground speed.
double GetWindWFpsIC(void) const
Gets the initial body axis Z wind velocity.
double GetBetaRadIC(void) const
Gets the initial angle of sideslip.
double GetLongitudeDegIC(void) const
Gets the initial longitude.
static char fgred[6]
red text
static char reset[5]
resets text properties
FGLocation holds an arbitrary location in the Earth centered Earth fixed reference frame (ECEF).
void SetEllipse(double semimajor, double semiminor)
Sets the semimajor and semiminor axis lengths for this planet.
const FGMatrix33 & GetTl2ec(void) const
Transform matrix from local horizontal to earth centered frame.
void SetLongitude(double longitude)
Set the longitude.
double GetLatitude() const
Get the GEOCENTRIC latitude in radians.
const FGMatrix33 & GetTec2l(void) const
Transform matrix from the earth centered to local horizontal frame.
double GetGeodLatitudeRad(void) const
Get the GEODETIC latitude in radians.
double GetLongitude() const
Get the longitude.
void SetLatitude(double latitude)
Set the GEOCENTRIC latitude.
double GetRadius() const
Get the distance from the center of the earth in feet.
double GetSeaLevelRadius(void) const
Get the sea level radius in feet below the current location.
void SetRadius(double radius)
Set the distance from the center of the earth.
double GetGeodAltitude(void) const
Gets the geodetic altitude in feet.
void SetPositionGeodetic(double lon, double lat, double height)
Sets the longitude, latitude and the distance above the reference spheroid.
Handles matrix math operations.
FGMatrix33 Transposed(void) const
Transposed matrix.
Models the Quaternion representation of rotations.
const FGColumnVector3 & GetEuler(void) const
Retrieves the Euler angles.
double GetSinEuler(int i) const
Retrieves sine of the given euler angle.
double GetCosEuler(int i) const
Retrieves cosine of the given euler angle.
const FGMatrix33 & GetT(void) const
Transformation matrix.
const FGMatrix33 & GetTInv(void) const
Backward transformation matrix.