From 88478f03d0bdd956d15b843cfba0f7025c9e827e Mon Sep 17 00:00:00 2001 From: ehofman Date: Sun, 10 May 2009 12:23:35 +0000 Subject: [PATCH] Sync. with JSBSim CVS --- src/FDM/JSBSim/models/FGExternalForce.cpp | 2 +- src/FDM/JSBSim/models/FGFCS.cpp | 21 +- src/FDM/JSBSim/models/FGLGear.cpp | 1 + src/FDM/JSBSim/models/FGModel.h | 1 + src/FDM/JSBSim/models/FGPropagate.cpp | 6 +- src/FDM/JSBSim/models/FGPropagate.h | 90 +++++--- .../models/flight_control/FGAccelerometer.cpp | 207 ++++++++++++++++++ .../models/flight_control/FGAccelerometer.h | 140 ++++++++++++ .../models/flight_control/FGFCSComponent.cpp | 2 + .../JSBSim/models/flight_control/FGSensor.cpp | 11 +- .../JSBSim/models/flight_control/FGSensor.h | 19 +- .../JSBSim/models/flight_control/Makefile.am | 4 +- src/FDM/JSBSim/models/propulsion/FGRocket.cpp | 1 + .../JSBSim/models/propulsion/FGTurboProp.cpp | 6 +- 14 files changed, 455 insertions(+), 56 deletions(-) create mode 100755 src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp create mode 100755 src/FDM/JSBSim/models/flight_control/FGAccelerometer.h diff --git a/src/FDM/JSBSim/models/FGExternalForce.cpp b/src/FDM/JSBSim/models/FGExternalForce.cpp index a9e13c9a1..349f28db3 100755 --- a/src/FDM/JSBSim/models/FGExternalForce.cpp +++ b/src/FDM/JSBSim/models/FGExternalForce.cpp @@ -150,7 +150,7 @@ FGExternalForce::FGExternalForce(const FGExternalForce& extForce) : FGForce(extF FGExternalForce::~FGExternalForce() { unbind( PropertyManager->GetNode("external_reactions")); - + delete Magnitude_Function; Debug(1); } diff --git a/src/FDM/JSBSim/models/FGFCS.cpp b/src/FDM/JSBSim/models/FGFCS.cpp index ac015f9b0..3c5b28ca5 100644 --- a/src/FDM/JSBSim/models/FGFCS.cpp +++ b/src/FDM/JSBSim/models/FGFCS.cpp @@ -54,6 +54,7 @@ INCLUDES #include #include #include +#include namespace JSBSim { @@ -453,7 +454,7 @@ void FGFCS::SetMixturePos(int engineNum, double setting) if (engineNum < (int)ThrottlePos.size()) { if (engineNum < 0) { - for (ctr=0;ctr<=MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr]; + for (ctr=0;ctrFindElement("property"); - if (property_element) cout << endl << " Declared properties" << endl << endl; + if (property_element && debug_lvl > 0) cout << endl << " Declared properties" << endl << endl; while (property_element) { interface_property_string = property_element->GetDataLine(); if (PropertyManager->HasNode(interface_property_string)) { - cout << " Property " << interface_property_string << " is already defined." << endl; + cerr << " Property " << interface_property_string << " is already defined." << endl; } else { double value=0.0; if ( ! property_element->GetAttributeValue("value").empty()) @@ -590,7 +591,8 @@ bool FGFCS::Load(Element* el, SystemType systype) interface_properties.push_back(new double(value)); interface_property_string = property_element->GetDataLine(); PropertyManager->Tie(interface_property_string, interface_properties.back()); - cout << " " << interface_property_string << " (initial value: " << value << ")" << endl; + if (debug_lvl > 0) + cout << " " << interface_property_string << " (initial value: " << value << ")" << endl; } property_element = document->FindNextElement("property"); } @@ -611,8 +613,9 @@ bool FGFCS::Load(Element* el, SystemType systype) interface_property_string = property_element->GetDataLine(); if (PropertyManager->HasNode(interface_property_string)) { FGPropertyManager* node = PropertyManager->GetNode(interface_property_string); - cout << " " << "Overriding value for property " << interface_property_string - << " (old value: " << node->getDoubleValue() << " new value: " << value << ")" << endl; + if (debug_lvl > 0) + cout << " " << "Overriding value for property " << interface_property_string + << " (old value: " << node->getDoubleValue() << " new value: " << value << ")" << endl; node->setDoubleValue(value); } else { interface_properties.push_back(new double(value)); @@ -679,6 +682,8 @@ bool FGFCS::Load(Element* el, SystemType systype) Components->push_back(new FGActuator(this, component_element)); } else if (component_element->GetName() == string("sensor")) { Components->push_back(new FGSensor(this, component_element)); + } else if (component_element->GetName() == string("accelerometer")) { + Components->push_back(new FGAccelerometer(this, component_element)); } else { cerr << "Unknown FCS component: " << component_element->GetName() << endl; } diff --git a/src/FDM/JSBSim/models/FGLGear.cpp b/src/FDM/JSBSim/models/FGLGear.cpp index 8ea49d207..f740cf643 100644 --- a/src/FDM/JSBSim/models/FGLGear.cpp +++ b/src/FDM/JSBSim/models/FGLGear.cpp @@ -260,6 +260,7 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number) : FGLGear::~FGLGear() { + delete ForceY_Table; Debug(1); } diff --git a/src/FDM/JSBSim/models/FGModel.h b/src/FDM/JSBSim/models/FGModel.h index 369a5255c..a28c77897 100644 --- a/src/FDM/JSBSim/models/FGModel.h +++ b/src/FDM/JSBSim/models/FGModel.h @@ -110,6 +110,7 @@ public: virtual bool InitModel(void); virtual void SetRate(int tt) {rate = tt;} virtual int GetRate(void) {return rate;} + FGFDMExec* GetExec(void) {return FDMExec;} void SetPropertyManager(FGPropertyManager *fgpm) { PropertyManager=fgpm;} diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index 65e57f017..8a41ec21e 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -57,8 +57,8 @@ INCLUDES #include #include "FGPropagate.h" -#include #include +#include #include "FGAircraft.h" #include "FGMassBalance.h" #include "FGInertial.h" @@ -511,6 +511,10 @@ void FGPropagate::bind(void) PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR); PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR); + PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi); + PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi); + PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi); + PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude); PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot); diff --git a/src/FDM/JSBSim/models/FGPropagate.h b/src/FDM/JSBSim/models/FGPropagate.h index 7446c38e2..adf136e79 100644 --- a/src/FDM/JSBSim/models/FGPropagate.h +++ b/src/FDM/JSBSim/models/FGPropagate.h @@ -40,7 +40,6 @@ INCLUDES #include #include -#include #include #include #include @@ -57,6 +56,8 @@ FORWARD DECLARATIONS namespace JSBSim { +class FGInitialCondition; + /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS DOCUMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -97,6 +98,27 @@ CLASS DECLARATION class FGPropagate : public FGModel { public: + + /** The current vehicle state vector structure contains the translational and + angular position, and the translational and angular velocity. */ + struct VehicleState { + /** Represents the current location of the vehicle in Earth centered Earth + fixed (ECEF) frame. + units ft */ + FGLocation vLocation; + /** The velocity vector of the vehicle with respect to the ECEF frame, + expressed in the body system. + units ft/sec */ + FGColumnVector3 vUVW; + /** The angular velocity vector for the vehicle relative to the ECEF frame, + expressed in the body frame. + units rad/sec */ + FGColumnVector3 vPQR; + /** The current orientation of the vehicle, that is, the orientation of the + body frame relative to the local, vehilce-carried, NED frame. */ + FGQuaternion vQtrn; + }; + /** Constructor. The constructor initializes several variables, and sets the initial set of integrators to use as follows: @@ -113,26 +135,6 @@ public: /// These define the indices use to select the various integrators. enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3}; -/** The current vehicle state vector structure contains the translational and - angular position, and the translational and angular velocity. */ -struct VehicleState { - /** Represents the current location of the vehicle in Earth centered Earth - fixed (ECEF) frame. - units ft */ - FGLocation vLocation; - /** The velocity vector of the vehicle with respect to the ECEF frame, - expressed in the body system. - units ft/sec */ - FGColumnVector3 vUVW; - /** The angular velocity vector for the vehicle relative to the ECEF frame, - expressed in the body frame. - units rad/sec */ - FGColumnVector3 vPQR; - /** The current orientation of the vehicle, that is, the orientation of the - body frame relative to the local, vehilce-carried, NED frame. */ - FGQuaternion vQtrn; -}; - /** Initializes the FGPropagate class after instantiation and prior to first execution. The base class FGModel::InitModel is called first, initializing pointers to the other FGModel objects (and others). */ @@ -181,7 +183,7 @@ struct VehicleState { */ const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; } - /** Retrieves the body angular rates vector. + /** Retrieves the body angular rates vector, relative to the ECEF frame. Retrieves the body angular rates (p, q, r), which are calculated by integration of the angular acceleration. The vector returned is represented by an FGColumnVector reference. The vector @@ -195,6 +197,20 @@ struct VehicleState { */ const FGColumnVector3& GetPQR(void) const {return VState.vPQR;} + /** Retrieves the body angular rates vector, relative to the ECI (inertial) frame. + Retrieves the body angular rates (p, q, r), which are calculated by integration + of the angular acceleration. + The vector returned is represented by an FGColumnVector reference. The vector + for the angular velocity in Body frame is organized (P, Q, R). The vector + is 1-based, so that the first element can be retrieved using the "()" operator. + In other words, vPQR(1) is P. Various convenience enumerators are defined + in FGJSBBase. The relevant enumerators for the vector returned by this call are, + eP=1, eQ=2, eR=3. + units rad/sec + @return The body frame angular rates in rad/sec. + */ + const FGColumnVector3& GetPQRi(void) const {return vPQRi;} + /** Retrieves the body axis angular acceleration vector. Retrieves the body axis angular acceleration vector in rad/sec^2. The angular acceleration vector is determined from the applied forces and @@ -286,7 +302,7 @@ struct VehicleState { */ double Gethmeters(void) const { return Geth()*fttom;} - /** Retrieves a body frame angular velocity component. + /** Retrieves a body frame angular velocity component relative to the ECEF frame. Retrieves a body frame angular velocity component. The angular velocity returned is extracted from the vPQR vector (an FGColumnVector). The vector for the angular velocity in Body frame is organized (P, Q, R). The vector @@ -299,6 +315,19 @@ struct VehicleState { */ double GetPQR(int axis) const {return VState.vPQR(axis);} + /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame. + Retrieves a body frame angular velocity component. The angular velocity + returned is extracted from the vPQR vector (an FGColumnVector). The vector + for the angular velocity in Body frame is organized (P, Q, R). The vector + is 1-based. In other words, GetPQR(1) returns P (roll rate). Various + convenience enumerators are defined in FGJSBBase. The relevant enumerators + for the angular velocity returned by this call are, eP=1, eQ=2, eR=3. + units rad/sec + @param axis the index of the angular velocity component desired (1-based). + @return The body frame angular velocity component. + */ + double GetPQRi(int axis) const {return vPQRi(axis);} + /** Retrieves a body frame angular acceleration component. Retrieves a body frame angular acceleration component. The angular acceleration returned is extracted from the vPQRdot vector (an @@ -423,13 +452,13 @@ struct VehicleState { @return a reference to the local-to-ECEF matrix. */ const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); } - const VehicleState GetVState(void) const { return VState; } + VehicleState* GetVState(void) { return &VState; } - void SetVState(VehicleState vstate) { - VState.vLocation = vstate.vLocation; - VState.vUVW = vstate.vUVW; - VState.vPQR = vstate.vPQR; - VState.vQtrn = vstate.vQtrn; // ... mmh + void SetVState(VehicleState* vstate) { + VState.vLocation = vstate->vLocation; + VState.vUVW = vstate->vUVW; + VState.vPQR = vstate->vPQR; + VState.vQtrn = vstate->vQtrn; // ... mmh } const FGQuaternion GetQuaternion(void) const { return VState.vQtrn; } @@ -504,4 +533,7 @@ private: }; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +#include + #endif diff --git a/src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp b/src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp new file mode 100755 index 000000000..af9ca54b2 --- /dev/null +++ b/src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp @@ -0,0 +1,207 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGAccelerometer.cpp + Author: Jon Berndt + Date started: 9 July 2005 + + ------------- Copyright (C) 2005 ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser General Public License as published by the Free Software + Foundation; either version 2 of the License, or (at your option) any later + version. + + This program is distributed in the hope that it will be useful, but WITHOUT + ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS + FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser General Public License along with + this program; if not, write to the Free Software Foundation, Inc., 59 Temple + Place - Suite 330, Boston, MA 02111-1307, USA. + + Further information about the GNU Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- + +HISTORY +-------------------------------------------------------------------------------- + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +COMMENTS, REFERENCES, and NOTES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGAccelerometer.h" + +namespace JSBSim { + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_ACCELEROMETER; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + + +FGAccelerometer::FGAccelerometer(FGFCS* fcs, Element* element) : FGSensor(fcs, element) +{ + Propagate = fcs->GetExec()->GetPropagate(); + MassBalance = fcs->GetExec()->GetMassBalance(); + + Element* location_element = element->FindElement("location"); + if (location_element) vLocation = location_element->FindElementTripletConvertTo("IN"); + else {cerr << "No location given for accelerometer. " << endl; exit(-1);} + + vRadius = MassBalance->StructuralToBody(vLocation); + + Element* orient_element = element->FindElement("orientation"); + if (orient_element) vOrient = orient_element->FindElementTripletConvertTo("RAD"); + else {cerr << "No orientation given for accelerometer. " << endl;} + + Element* axis_element = element->FindElement("axis"); + if (axis_element) { + string sAxis = element->FindElementValue("axis"); + if (sAxis == "X" || sAxis == "x") { + axis = 1; + } else if (sAxis == "Y" || sAxis == "y") { + axis = 2; + } else if (sAxis == "Z" || sAxis == "z") { + axis = 3; + } else { + cerr << " Incorrect/no axis specified for accelerometer; assuming X axis" << endl; + axis = 1; + } + } + + CalculateTransformMatrix(); + + Debug(0); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGAccelerometer::~FGAccelerometer() +{ + Debug(1); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGAccelerometer::Run(void ) +{ + // There is no input assumed. This is a dedicated acceleration sensor. + + vRadius = MassBalance->StructuralToBody(vLocation); + + vAccel = mT * (Propagate->GetUVWdot() + + Propagate->GetPQRdot() * vRadius + + Propagate->GetPQR() * (Propagate->GetPQR() * vRadius)); + + Input = vAccel(axis); + + Output = Input; // perfect accelerometer + + // Degrade signal as specified + + if (fail_stuck) { + Output = PreviousOutput; + return true; + } + + if (lag != 0.0) Lag(); // models accelerometer lag + if (noise_variance != 0.0) Noise(); // models noise + if (drift_rate != 0.0) Drift(); // models drift over time + if (bias != 0.0) Bias(); // models a finite bias + + if (fail_low) Output = -HUGE_VAL; + if (fail_high) Output = HUGE_VAL; + + if (bits != 0) Quantize(); // models quantization degradation +// if (delay != 0.0) Delay(); // models system signal transport latencies + + Clip(); // Is it right to clip an accelerometer? + return true; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGAccelerometer::CalculateTransformMatrix(void) +{ + double cp,sp,cr,sr,cy,sy; + + cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch)); + cr=cos(vOrient(eRoll)); sr=sin(vOrient(eRoll)); + cy=cos(vOrient(eYaw)); sy=sin(vOrient(eYaw)); + + mT(1,1) = cp*cy; + mT(1,2) = cp*sy; + mT(1,3) = -sp; + + mT(2,1) = sr*sp*cy - cr*sy; + mT(2,2) = sr*sp*sy + cr*cy; + mT(2,3) = sr*cp; + + mT(3,1) = cr*sp*cy + sr*sy; + mT(3,2) = cr*sp*sy - sr*cy; + mT(3,3) = cr*cp; + + // This transform is different than for FGForce, where we want a native nozzle + // force in body frame. Here we calculate the body frame accel and want it in + // the transformed accelerometer frame. So, the next line is commented out. + // mT = mT.Inverse(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// The bitmasked value choices are as follows: +// unset: In this case (the default) JSBSim would only print +// out the normally expected messages, essentially echoing +// the config files as they are read. If the environment +// variable is not set, debug_lvl is set to 1 internally +// 0: This requests JSBSim not to output any messages +// whatsoever. +// 1: This value explicity requests the normal JSBSim +// startup messages +// 2: This value asks for a message to be printed out when +// a class is instantiated +// 4: When this value is set, a message is displayed when a +// FGModel object executes its Run() method +// 8: When this value is set, various runtime state variables +// are printed out periodically +// 16: When set various parameters are sanity checked and +// a message is printed out when they go out of bounds + +void FGAccelerometer::Debug(int from) +{ + string ax[4] = {"none", "X", "Y", "Z"}; + + if (debug_lvl <= 0) return; + + if (debug_lvl & 1) { // Standard console startup message output + if (from == 0) { // Constructor + cout << " Axis: " << ax[axis] << endl; + } + } + if (debug_lvl & 2 ) { // Instantiation/Destruction notification + if (from == 0) cout << "Instantiated: FGAccelerometer" << endl; + if (from == 1) cout << "Destroyed: FGAccelerometer" << 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 (debug_lvl & 64) { + if (from == 0) { // Constructor + cout << IdSrc << endl; + cout << IdHdr << endl; + } + } +} +} diff --git a/src/FDM/JSBSim/models/flight_control/FGAccelerometer.h b/src/FDM/JSBSim/models/flight_control/FGAccelerometer.h new file mode 100755 index 000000000..9bdcece7d --- /dev/null +++ b/src/FDM/JSBSim/models/flight_control/FGAccelerometer.h @@ -0,0 +1,140 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Header: FGAccelerometer.h + Author: Jon Berndt + Date started: May 2009 + + ------------- Copyright (C) 2009 ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser General Public License as published by the Free Software + Foundation; either version 2 of the License, or (at your option) any later + version. + + This program is distributed in the hope that it will be useful, but WITHOUT + ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS + FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser General Public License along with + this program; if not, write to the Free Software Foundation, Inc., 59 Temple + Place - Suite 330, Boston, MA 02111-1307, USA. + + Further information about the GNU Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +HISTORY +-------------------------------------------------------------------------------- + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef FGACCELEROMETER_H +#define FGACCELEROMETER_H + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGSensor.h" +#include +#include "models/FGPropagate.h" +#include "models/FGMassBalance.h" +#include "math/FGColumnVector3.h" +#include "math/FGMatrix33.h" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_ACCELEROMETER "$Id$" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +namespace JSBSim { + +class FGFCS; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DOCUMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/** Encapsulates a Accelerometer component for the flight control system. + +Syntax: + +@code + + + number + number + + number + number + number + + number + number + +@endcode + +Example: + +@code + + + 0.5 + 2 + + 12 + 0 + 400 + + 0.5 + +@endcode + +The only required element in the accelerometer definition is the input element. In that +case, no degradation would be modeled, and the output would simply be the input. + +For noise, if the type is PERCENT, then the value supplied is understood to be a +percentage variance. That is, if the number given is 0.05, the the variance is +understood to be +/-0.05 percent maximum variance. So, the actual value for the accelerometer +will be *anywhere* from 0.95 to 1.05 of the actual "perfect" value at any time - +even varying all the way from 0.95 to 1.05 in adjacent frames - whatever the delta +time. + +@author Jon S. Berndt +@version $Revision$ +*/ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DECLARATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGAccelerometer : public FGSensor +{ +public: + FGAccelerometer(FGFCS* fcs, Element* element); + ~FGAccelerometer(); + + bool Run (void); + +private: + FGPropagate* Propagate; + FGMassBalance* MassBalance; + FGColumnVector3 vLocation; + FGColumnVector3 vOrient; + FGColumnVector3 vRadius; + FGColumnVector3 vAccel; + FGMatrix33 mT; + void CalculateTransformMatrix(void); + int axis; + + void Debug(int from); +}; +} +#endif diff --git a/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp b/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp index 26f73facf..7d4829357 100644 --- a/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp +++ b/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp @@ -89,6 +89,8 @@ FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs) Type = "PID"; } else if (element->GetName() == string("sensor")) { Type = "SENSOR"; + } else if (element->GetName() == string("accelerometer")) { + Type = "ACCELEROMETER"; } else if (element->GetName() == string("actuator")) { Type = "ACTUATOR"; } else { // illegal component in this channel diff --git a/src/FDM/JSBSim/models/flight_control/FGSensor.cpp b/src/FDM/JSBSim/models/flight_control/FGSensor.cpp index 20a587e28..5e4ede578 100755 --- a/src/FDM/JSBSim/models/flight_control/FGSensor.cpp +++ b/src/FDM/JSBSim/models/flight_control/FGSensor.cpp @@ -252,11 +252,12 @@ void FGSensor::Debug(int from) if (debug_lvl & 1) { // Standard console startup message output if (from == 0) { // Constructor - if (InputSigns[0] < 0) - cout << " INPUT: -" << InputNodes[0]->getName() << endl; - else - cout << " INPUT: " << InputNodes[0]->getName() << endl; - + if (InputSigns.size() > 0) { + if (InputSigns[0] < 0) + cout << " INPUT: -" << InputNodes[0]->getName() << endl; + else + cout << " INPUT: " << InputNodes[0]->getName() << endl; + } if (IsOutput) cout << " OUTPUT: " << OutputNode->getName() << endl; if (bits != 0) { if (quant_property.empty()) diff --git a/src/FDM/JSBSim/models/flight_control/FGSensor.h b/src/FDM/JSBSim/models/flight_control/FGSensor.h index fddede857..87fd3d518 100755 --- a/src/FDM/JSBSim/models/flight_control/FGSensor.h +++ b/src/FDM/JSBSim/models/flight_control/FGSensor.h @@ -117,18 +117,18 @@ public: FGSensor(FGFCS* fcs, Element* element); ~FGSensor(); - inline void SetFailLow(double val) {if (val > 0.0) fail_low = true; else fail_low = false;} - inline void SetFailHigh(double val) {if (val > 0.0) fail_high = true; else fail_high = false;} - inline void SetFailStuck(double val) {if (val > 0.0) fail_stuck = true; else fail_stuck = false;} + void SetFailLow(double val) {if (val > 0.0) fail_low = true; else fail_low = false;} + void SetFailHigh(double val) {if (val > 0.0) fail_high = true; else fail_high = false;} + void SetFailStuck(double val) {if (val > 0.0) fail_stuck = true; else fail_stuck = false;} - inline double GetFailLow(void) const {if (fail_low) return 1.0; else return 0.0;} - inline double GetFailHigh(void) const {if (fail_high) return 1.0; else return 0.0;} - inline double GetFailStuck(void) const {if (fail_stuck) return 1.0; else return 0.0;} - inline int GetQuantized(void) const {return quantized;} + double GetFailLow(void) const {if (fail_low) return 1.0; else return 0.0;} + double GetFailHigh(void) const {if (fail_high) return 1.0; else return 0.0;} + double GetFailStuck(void) const {if (fail_stuck) return 1.0; else return 0.0;} + int GetQuantized(void) const {return quantized;} - bool Run (void); + virtual bool Run (void); -private: +protected: enum eNoiseType {ePercent=0, eAbsolute} NoiseType; double dt; double min, max; @@ -160,6 +160,7 @@ private: void bind(void); +private: void Debug(int from); }; } diff --git a/src/FDM/JSBSim/models/flight_control/Makefile.am b/src/FDM/JSBSim/models/flight_control/Makefile.am index 5ccfe5cae..376761b5a 100644 --- a/src/FDM/JSBSim/models/flight_control/Makefile.am +++ b/src/FDM/JSBSim/models/flight_control/Makefile.am @@ -3,10 +3,10 @@ noinst_LIBRARIES = libFlightControl.a libFlightControl_a_SOURCES = FGPID.cpp FGDeadBand.cpp FGFCSComponent.cpp \ FGFilter.cpp FGGain.cpp FGGradient.cpp FGKinemat.cpp \ FGSummer.cpp FGSwitch.cpp FGFCSFunction.cpp FGSensor.cpp \ - FGActuator.cpp + FGActuator.cpp FGAccelerometer.cpp noinst_HEADERS = FGPID.h FGDeadBand.h FGFCSComponent.h FGFilter.h \ FGGain.h FGGradient.h FGKinemat.h FGSummer.h FGSwitch.h FGFCSFunction.h \ - FGSensor.h FGActuator.h + FGSensor.h FGActuator.h FGAccelerometer.h INCLUDES = -I$(top_srcdir)/src/FDM/JSBSim diff --git a/src/FDM/JSBSim/models/propulsion/FGRocket.cpp b/src/FDM/JSBSim/models/propulsion/FGRocket.cpp index f6716dca3..479dc1cc5 100644 --- a/src/FDM/JSBSim/models/propulsion/FGRocket.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGRocket.cpp @@ -98,6 +98,7 @@ FGRocket::FGRocket(FGFDMExec* exec, Element *el, int engine_number) FGRocket::~FGRocket(void) { + delete ThrustTable; Debug(1); } diff --git a/src/FDM/JSBSim/models/propulsion/FGTurboProp.cpp b/src/FDM/JSBSim/models/propulsion/FGTurboProp.cpp index 893c994e6..3030a16a6 100755 --- a/src/FDM/JSBSim/models/propulsion/FGTurboProp.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGTurboProp.cpp @@ -57,7 +57,8 @@ CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ FGTurboProp::FGTurboProp(FGFDMExec* exec, Element *el, int engine_number) - : FGEngine(exec, el, engine_number) + : FGEngine(exec, el, engine_number), + ITT_N1(NULL), EnginePowerRPM_N1(NULL), EnginePowerVC(NULL) { SetDefaults(); @@ -69,6 +70,9 @@ FGTurboProp::FGTurboProp(FGFDMExec* exec, Element *el, int engine_number) FGTurboProp::~FGTurboProp() { + delete ITT_N1; + delete EnginePowerRPM_N1; + delete EnginePowerVC; Debug(1); } -- 2.39.5