void FGPropagate::Debug(int from)
{
if (debug_lvl <= 0) return;
if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) { // Constructor
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
if (from == 0) cout << "Instantiated: FGPropagate" << endl;
if (from == 1) cout << "Destroyed: FGPropagate" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 ) { // Runtime state variables
}
if (debug_lvl & 16) { // Sanity checking
if (from == 2) { // State sanity checking
if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
exit(-1);
}
if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
exit(-1);
}
if (fabs(GetDistanceAGL()) > 1e10) {
cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
exit(-1);
}
}
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
cout << IdSrc << endl;
cout << IdHdr << endl;
}
}
}
void FGPropagate::Debug(int from)
{
if (debug_lvl <= 0) return;
if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) { // Constructor
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
if (from == 0) cout << "Instantiated: FGPropagate" << endl;
if (from == 1) cout << "Destroyed: FGPropagate" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 && from == 2) { // Runtime state variables
cout << endl << fgblue << highint << left
<< " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
<< reset << endl;
cout << endl;
cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
<< GetEarthPositionAngleDeg() << endl;
cout << endl;
cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
// cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
cout << endl;
cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
<< reset << endl << Tec2b.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
<< reset << endl << Tb2ec.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
<< reset << endl << Tl2b.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
<< reset << endl << Tb2l.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
<< reset << endl << Tl2ec.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
<< reset << endl << Tec2l.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
<< reset << endl << Tec2i.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
<< reset << endl << Ti2ec.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
<< reset << endl << Ti2b.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
<< reset << endl << Tb2i.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
<< reset << endl << Ti2l.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
<< reset << endl << Tl2i.Dump("\t", " ") << endl;
cout << highint << " Associated Euler angles (deg): " << setw(8)
<< setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
<< endl << endl;
cout << setprecision(6); // reset the output stream
}
if (debug_lvl & 16) { // Sanity checking
if (from == 2) { // State sanity checking
if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
exit(-1);
}
if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
exit(-1);
}
if (fabs(GetDistanceAGL()) > 1e10) {
cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
exit(-1);
}
}
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
cout << IdSrc << endl;
cout << IdHdr << endl;
}
}
}
void FGPropagate::WriteStateFile(int num)
{
ofstream outfile;
if (num == 0) return;
string filename = FDMExec->GetFullAircraftPath();
string sim_time = to_string((double)FDMExec->GetSimTime());
if (filename.empty()) filename = "initfile.";
else filename.append("/initfile.");
// Append sim time to the filename since there may be more than one created during a simulation run
filename += to_string((double)FDMExec->GetSimTime())+".xml";
switch(num) {
case 1:
outfile.open(filename.c_str());
if (outfile.is_open()) {
outfile << "<?xml version=\"1.0\"?>" << endl;
outfile << "<initialize name=\"reset00\">" << endl;
outfile << " <ubody unit=\"FT/SEC\"> " << VState.vUVW(eU) << " </ubody> " << endl;
outfile << " <vbody unit=\"FT/SEC\"> " << VState.vUVW(eV) << " </vbody> " << endl;
outfile << " <wbody unit=\"FT/SEC\"> " << VState.vUVW(eW) << " </wbody> " << endl;
outfile << " <phi unit=\"DEG\"> " << VState.qAttitudeLocal.GetEuler(ePhi)*radtodeg << " </phi>" << endl;
outfile << " <theta unit=\"DEG\"> " << VState.qAttitudeLocal.GetEuler(eTht)*radtodeg << " </theta>" << endl;
outfile << " <psi unit=\"DEG\"> " << VState.qAttitudeLocal.GetEuler(ePsi)*radtodeg << " </psi>" << endl;
outfile << " <longitude unit=\"DEG\"> " << VState.vLocation.GetLongitudeDeg() << " </longitude>" << endl;
outfile << " <latitude unit=\"DEG\"> " << VState.vLocation.GetLatitudeDeg() << " </latitude>" << endl;
outfile << " <altitude unit=\"FT\"> " << GetDistanceAGL() << " </altitude>" << endl;
outfile << "</initialize>" << endl;
outfile.close();
} else {
cerr << "Could not open and/or write the state to the initial conditions file: " << filename << endl;
}
break;
case 2:
outfile.open(filename.c_str());
if (outfile.is_open()) {
outfile << "<?xml version=\"1.0\"?>" << endl;
outfile << "<initialize name=\"IC File\" version=\"2.0\">" << endl;
outfile << "" << endl;
outfile << " <position frame=\"ECEF\">" << endl;
outfile << " <latitude unit=\"DEG\" type=\"geodetic\"> " << VState.vLocation.GetGeodLatitudeDeg() << " </latitude>" << endl;
outfile << " <longitude unit=\"DEG\"> " << VState.vLocation.GetLongitudeDeg() << " </longitude>" << endl;
outfile << " <altitudeMSL unit=\"FT\"> " << GetAltitudeASL() << " </altitudeMSL>" << endl;
outfile << " </position>" << endl;
outfile << "" << endl;
outfile << " <orientation unit=\"DEG\" frame=\"LOCAL\">" << endl;
outfile << " <yaw> " << VState.qAttitudeLocal.GetEulerDeg(eYaw) << " </yaw>" << endl;
outfile << " <pitch> " << VState.qAttitudeLocal.GetEulerDeg(ePitch) << " </pitch>" << endl;
outfile << " <roll> " << VState.qAttitudeLocal.GetEulerDeg(eRoll) << " </roll>" << endl;
outfile << " </orientation>" << endl;
outfile << "" << endl;
outfile << " <velocity unit=\"FT/SEC\" frame=\"LOCAL\">" << endl;
outfile << " <x> " << GetVel(eNorth) << " </x>" << endl;
outfile << " <y> " << GetVel(eEast) << " </y>" << endl;
outfile << " <z> " << GetVel(eDown) << " </z>" << endl;
outfile << " </velocity>" << endl;
outfile << "" << endl;
outfile << " <attitude_rate unit=\"DEG/SEC\" frame=\"BODY\">" << endl;
outfile << " <roll> " << (VState.vPQR*radtodeg)(eRoll) << " </roll>" << endl;
outfile << " <pitch> " << (VState.vPQR*radtodeg)(ePitch) << " </pitch>" << endl;
outfile << " <yaw> " << (VState.vPQR*radtodeg)(eYaw) << " </yaw>" << endl;
outfile << " </attitude_rate>" << endl;
outfile << "" << endl;
outfile << "</initialize>" << endl;
outfile.close();
} else {
cerr << "Could not open and/or write the state to the initial conditions file: " << filename << endl;
}
break;
default:
cerr << "When writing a state file, the supplied value must be 1 or 2 for the version number of the resulting IC file" << endl;
}
}
