diff --git a/QtRocket.cpp b/QtRocket.cpp index 49a8bc5..1fd1e9d 100644 --- a/QtRocket.cpp +++ b/QtRocket.cpp @@ -81,7 +81,7 @@ QtRocket::QtRocket() setEnvironment(std::make_shared()); rocket.first = - std::make_shared(); + std::make_shared(); rocket.second = std::make_shared(rocket.first); diff --git a/QtRocket.h b/QtRocket.h index 7852928..d1c124b 100644 --- a/QtRocket.h +++ b/QtRocket.h @@ -44,13 +44,13 @@ public: std::shared_ptr getEnvironment() { return environment; } void setTimeStep(double t) { rocket.second->setTimeStep(t); } - std::shared_ptr getRocket() { return rocket.first; } + std::shared_ptr getRocket() { return rocket.first; } std::shared_ptr getMotorDatabase() { return motorDatabase; } void addMotorModels(std::vector& m); - void addRocket(std::shared_ptr r) { rocket.first = r; } + void addRocket(std::shared_ptr r) { rocket.first = r; } void setEnvironment(std::shared_ptr e) { environment = e; } @@ -79,7 +79,7 @@ private: utils::Logger* logger; - std::pair, std::shared_ptr> rocket; + std::pair, std::shared_ptr> rocket; std::shared_ptr environment; std::shared_ptr motorDatabase; diff --git a/gui/AnalysisWindow.cpp b/gui/AnalysisWindow.cpp index de60f10..8b8fbda 100644 --- a/gui/AnalysisWindow.cpp +++ b/gui/AnalysisWindow.cpp @@ -91,8 +91,8 @@ void AnalysisWindow::onButton_plotVelocity_clicked() void AnalysisWindow::onButton_plotMotorCurve_clicked() { - std::shared_ptr rocket = QtRocket::getInstance()->getRocket(); - model::MotorModel motor = rocket->getCurrentMotorModel(); + std::shared_ptr rocket = QtRocket::getInstance()->getRocket(); + model::MotorModel& motor = rocket->getCurrentStage()->getMotorModel(); ThrustCurve tc = motor.getThrustCurve(); diff --git a/model/CMakeLists.txt b/model/CMakeLists.txt index 72df3c1..c943867 100644 --- a/model/CMakeLists.txt +++ b/model/CMakeLists.txt @@ -5,8 +5,12 @@ add_library(model MotorModelDatabase.h Part.cpp Part.h + Propagatable.cpp + Propagatable.h Rocket.cpp Rocket.h + Stage.cpp + Stage.h ThrustCurve.cpp ThrustCurve.h) diff --git a/model/Part.cpp b/model/Part.cpp index fd3e890..be05350 100644 --- a/model/Part.cpp +++ b/model/Part.cpp @@ -1,6 +1,103 @@ #include "Part.h" +#include "utils/Logger.h" namespace model { +Part::Part() +{} + +Part::~Part() +{} + +Part::Part(const Part& orig) + : parent(orig.parent), + name(orig.name), + inertiaTensor(orig.inertiaTensor), + compositeInertiaTensor(orig.compositeInertiaTensor), + mass(orig.mass), + compositeMass(orig.compositeMass), + cm(orig.cm), + length(orig.length), + innerRadiusTop(orig.innerRadiusTop), + outerRadiusTop(orig.outerRadiusTop), + innerRadiusBottom(orig.innerRadiusBottom), + outerRadiusBottom(orig.outerRadiusBottom), + needsRecomputing(orig.needsRecomputing), + childParts() +{ + + // We are copying the whole tree. If the part we're copying itself has child + // parts, we are also copying all of them! This may be inefficient and not what + // is desired, but it is less likely to lead to weird bugs with the same part + // appearing in multiple locations of the rocket + utils::Logger::getInstance()->debug("Calling model::Part copy constructor. Recursively copying all child parts. Check Part names for uniqueness"); + + + for(const auto& i : orig.childParts) + { + Part& x = *std::get<0>(i); + std::shared_ptr tempPart = std::make_shared(x); + childParts.emplace_back(tempPart, std::get<1>(i));; + } + +} + + +double Part::getChildMasses(double t) +{ + double childMasses{0.0}; + for(const auto& i : childParts) + { + childMasses += std::get<0>(i)->getMass(t); + } + return childMasses; + +} + +void Part::addChildPart(const Part& childPart, Vector3 position) +{ + + double childMass = childPart.compositeMass; + Matrix3 childInertiaTensor = childPart.compositeInertiaTensor; + std::shared_ptr newChild = std::make_shared(childPart); + // Set the parent pointer + newChild->parent = this; + + // Recompute inertia tensor + + childInertiaTensor += childMass * ( position.dot(position) * Matrix3::Identity() - position*position.transpose()); + + compositeInertiaTensor += childInertiaTensor; + compositeMass += childMass; + + childParts.emplace_back(std::move(newChild), std::move(position)); + + if(parent) + { + parent->markAsNeedsRecomputing(); + parent->recomputeInertiaTensor(); + } + +} + +void Part::recomputeInertiaTensor() +{ + if(!needsRecomputing) + { + return; + } + // recompute the whole composite inertia tensor + // Reset the composite inertia tensor + compositeInertiaTensor = inertiaTensor; + compositeMass = mass; + for(auto& [child, pos] : childParts) + { + child->recomputeInertiaTensor(); + compositeInertiaTensor += child->compositeInertiaTensor + child->compositeMass * ( pos.dot(pos) * Matrix3::Identity() - pos*pos.transpose()); + compositeMass += child->compositeMass; + } + needsRecomputing = false; +} + } // namespace model \ No newline at end of file diff --git a/model/Part.h b/model/Part.h index e766fca..7926c21 100644 --- a/model/Part.h +++ b/model/Part.h @@ -4,16 +4,16 @@ /// \cond // C headers // C++ headers -#include -#include -#include +#include #include +#include // 3rd party headers /// \endcond // qtrocket headers #include "utils/math/MathTypes.h" +#include "model/Part.h" namespace model { @@ -24,8 +24,10 @@ public: Part(); virtual ~Part(); + Part(const Part&); + void setMass(double m) { mass = m; } - + // Set the inertia tensor void setI(const Matrix3& I) { inertiaTensor = I; } @@ -37,11 +39,58 @@ public: void setInnerRadius(double r) { innerRadiusTop = r; innerRadiusBottom = r; } void setOuterRadius(double r) { outerRadiusTop = r; outerRadiusBottom = r; } + + double getMass(double t) + { + return mass; + } + + double getCompositeMass(double t) + { + return compositeMass; + } + + /** + * @brief Add a child part to this part. + * + * @param childPart Child part to add + * @param position Relative position of the child part's center-of-mass w.r.t the + * parent's center of mass + */ + void addChildPart(const Part& childPart, Vector3 position); + + /** + * @brief Recomputes the inertia tensor. If the change is due to the change in inertia + * of a child part, an optional name of the child part can be given to + * only recompute that change rather than recompute all child inertia + * tensors + * + * @param name Optional name of the child part to recompute. If empty, it will + * recompute all child inertia tensors + */ + //void recomputeInertiaTensor(std::string name = ""); + void recomputeInertiaTensor(); private: + // This is a pointer to the parent Part, if it has one. Purpose is to be able to + // tell the parent if it needs to recompute anything if this part changes. e.g. + // if a change to this part's inertia tensor occurs, the parent needs to recompute + // it's total inertia tensor. + Part* parent{nullptr}; + + std::string name; + + double getChildMasses(double t); + void markAsNeedsRecomputing() + { needsRecomputing = true; if(parent) { parent->markAsNeedsRecomputing(); }} + + // Because a part is both a simple part and the composite of itself with all of it's children, + // we will keep track of this object's inertia tensor (without children), and the composite + // one with all of it's children attached Matrix3 inertiaTensor; // moment of inertia tensor with respect to the part's center of mass and - // + Matrix3 compositeInertiaTensor; double mass; // The moment of inertia tensor also has this, so don't double compute + double compositeMass; // The mass of this part along with all attached parts Vector3 cm; // center of mass wrt middle of component @@ -53,7 +102,11 @@ private: double innerRadiusBottom; double outerRadiusBottom; + bool needsRecomputing{false}; + /// @brief child parts and the relative positions of their center of mass w.r.t. + /// the center of mass of this part + std::vector, Vector3>> childParts; }; } diff --git a/model/Propagatable.cpp b/model/Propagatable.cpp new file mode 100644 index 0000000..e69de29 diff --git a/model/Propagatable.h b/model/Propagatable.h new file mode 100644 index 0000000..434665b --- /dev/null +++ b/model/Propagatable.h @@ -0,0 +1,40 @@ +#ifndef MODEL_PROPAGATABLE_H +#define MODEL_PROPAGATABLE_H + +/// \cond +// C headers +// C++ headers +#include +// 3rd party headers +/// \endcond + +// qtrocket headers +#include "sim/Aero.h" +#include "sim/StateData.h" + +namespace model +{ + +class Propagatable +{ +public: + Propagatable() {} + virtual ~Propagatable() {} + + virtual double getDragCoefficient() = 0; + virtual void setDragCoefficient(double d) = 0; + + virtual bool terminateCondition(const std::pair&) = 0; + + virtual double getMass(double t) = 0; + virtual double getThrust(double t) = 0; +private: + + sim::Aero aeroData; + + StateData state; +}; + +} + +#endif // MODEL_PROPAGATABLE_H \ No newline at end of file diff --git a/model/Rocket.cpp b/model/Rocket.cpp index 350dd65..3dafa4c 100644 --- a/model/Rocket.cpp +++ b/model/Rocket.cpp @@ -3,6 +3,9 @@ #include "Rocket.h" #include "QtRocket.h" +namespace model +{ + Rocket::Rocket() { @@ -10,12 +13,12 @@ Rocket::Rocket() void Rocket::launch() { - mm.startMotor(0.0); + currentStage->getMotorModel().startMotor(0.0); } void Rocket::setMotorModel(const model::MotorModel& motor) { - mm = motor; + currentStage->setMotorModel(motor); } bool Rocket::terminateCondition(const std::pair& cond) @@ -29,5 +32,17 @@ bool Rocket::terminateCondition(const std::pair& cond) double Rocket::getThrust(double t) { - return mm.getThrust(t); + return currentStage->getMotorModel().getThrust(t); } + +double Rocket::getMass(double t) +{ + double totalMass = 0.0; + for(const auto& stage : stages) + { + totalMass += stage.second->getMass(t); + } + return totalMass; +} + +} // namespace model \ No newline at end of file diff --git a/model/Rocket.h b/model/Rocket.h index d7c874f..a7f2120 100644 --- a/model/Rocket.h +++ b/model/Rocket.h @@ -4,6 +4,7 @@ /// \cond // C headers // C++ headers +#include #include #include #include // std::move @@ -13,15 +14,20 @@ // qtrocket headers #include "model/ThrustCurve.h" -#include "model/MotorModel.h" #include "sim/Propagator.h" #include "utils/math/MathTypes.h" +#include "model/Stage.h" +#include "model/Propagatable.h" + +namespace model +{ + /** * @brief The Rocket class holds all rocket components * */ -class Rocket +class Rocket : public Propagatable { public: /** @@ -29,40 +35,18 @@ public: */ Rocket(); + /** + * @brief Rocket class destructor + * + */ + virtual ~Rocket() {} + /** * @brief launch Propagates the Rocket object until termination, * normally when altitude crosses from positive to negative */ void launch(); - /** - * @brief getMass returns the current mass of the rocket. This is the sum of all components' masses - * @return total current mass of the Rocket - */ - double getMass(double simTime) const { return mass + mm.getMass(simTime); } - - /** - * @brief setMass sets the current total mass of the Rocket - * @param m total Rocket mass - * @todo This should be dynamically computed, not set. Fix this - */ - void setMass(double m) { mass = m;} - - /** - * @brief setDragCoefficient sets the current total drag coefficient of the Rocket - * @param d drag coefficient - * @todo This should be dynamically computed, not set. Fix this - */ - void setDragCoefficient(double d) { dragCoeff = d; } - - /** - * @brief getDragCoefficient returns the current drag coefficient - * - * This is intended to be called by the propagator during propagation. - * @return the coefficient of drag - */ - double getDragCoefficient() const { return dragCoeff; } - /** * @brief getThrust returns current motor thrust * @param t current simulation time @@ -70,6 +54,13 @@ public: */ double getThrust(double t); + /** + * @brief getMass returns current rocket + * @param t current simulation time + * @return mass in kg + */ + virtual double getMass(double t) override; + /** * @brief setMotorModel * @param motor @@ -80,14 +71,14 @@ public: * @brief Returns the current motor model. * @return The current motor model */ - const model::MotorModel& getCurrentMotorModel() const { return mm; } + //const model::MotorModel& getCurrentMotorModel() const { return mm; } /** * @brief terminateCondition returns true or false, whether the passed-in time/state matches the terminate condition * @param cond time/state pair * @return true if the passed-in time/state satisfies the terminate condition */ - bool terminateCondition(const std::pair& cond); + virtual bool terminateCondition(const std::pair& cond) override; /** * @brief setName sets the rocket name @@ -95,16 +86,22 @@ public: */ void setName(const std::string& n) { name = n; } + virtual double getDragCoefficient() override { return 1.0; } + virtual void setDragCoefficient(double d) override { } + void setMass(double m) { } + std::shared_ptr getCurrentStage() { return currentStage; } private: std::string name; /// Rocket name - double dragCoeff; /// @todo get rid of this, should be dynamically calculated - double mass; /// @todo get rid of this, should be dynamically computed, but is the current rocket mass - model::MotorModel mm; /// Current Motor Model + std::map> stages; + std::shared_ptr currentStage; + //model::MotorModel mm; /// Current Motor Model + }; +} // namespace model #endif // ROCKET_H diff --git a/model/Stage.cpp b/model/Stage.cpp new file mode 100644 index 0000000..ab6a0c1 --- /dev/null +++ b/model/Stage.cpp @@ -0,0 +1,27 @@ +/// \cond +// C headers +// C++ headers +#include +#include + +// 3rd party headers +/// \endcond + +// qtrocket headers +#include "Stage.h" + +namespace model +{ + +Stage::Stage() +{} + +Stage::~Stage() +{} + +void Stage::setMotorModel(const model::MotorModel& motor) +{ + mm = motor; +} + +} // namespace model \ No newline at end of file diff --git a/model/Stage.h b/model/Stage.h new file mode 100644 index 0000000..c3df69f --- /dev/null +++ b/model/Stage.h @@ -0,0 +1,69 @@ +#ifndef SIM_STAGE_H +#define SIM_STAGE_H + +/// \cond +// C headers +// C++ headers +#include + +// 3rd party headers +/// \endcond + +// qtrocket headers +#include "model/MotorModel.h" +#include "model/Part.h" +#include "model/Propagatable.h" + +namespace model +{ + +class Stage : public Propagatable +{ +public: + Stage(); + virtual ~Stage(); + + /** + * @brief setMotorModel + * @param motor + */ + void setMotorModel(const model::MotorModel& motor); + + /** + * @brief Returns the current motor model. + * @return The current motor model + */ + model::MotorModel& getMotorModel() { return mm; } + + virtual double getMass(double t) override + { + return topPart.getCompositeMass(t) + mm.getMass(t); + } + + virtual double getDragCoefficient() override { return 1.0 ;} + virtual void setDragCoefficient(double d) override {} + + virtual bool terminateCondition(const std::pair& cond) override + { + // Terminate propagation when the z coordinate drops below zero + if(cond.second.position[2] < 0) + return true; + else + return false; + } + + virtual double getThrust(double t) override { return mm.getThrust(t); } + + +private: + std::string name; + + Part topPart; + + model::MotorModel mm; + Vector3 motorModelPosition; // position of motor cg w.r.t. the stage c.g. +}; + +} // namespace model + +#endif // SIM_STAGE_H \ No newline at end of file diff --git a/sim/Aero.cpp b/sim/Aero.cpp new file mode 100644 index 0000000..e69de29 diff --git a/sim/Aero.h b/sim/Aero.h new file mode 100644 index 0000000..08636f7 --- /dev/null +++ b/sim/Aero.h @@ -0,0 +1,41 @@ +#ifndef SIM_AERO_H +#define SIM_AERO_H + +/// \cond +// C headers +// C++ headers +#include + +// 3rd party headers +/// \endcond + +// qtrocket headers +#include "utils/math/MathTypes.h" + +namespace sim +{ + +class Aero +{ +public: + +private: + + Vector3 cp; /// center of pressure + + double Cx; /// longitudinal coefficient + double Cy; /// These are probably the same for axial symmetric + double Cz; /// rockets. The coeffients in the y and z body directions + + double Cl; // roll moment coefficient + double Cm; // pitch moment coefficient + double Cn; // yaw moment coefficient + + double baseCd; // coefficient of drag due to base drag + double Cd; // total coeffient of drag + + +}; +} + +#endif // SIM_AERO_H \ No newline at end of file diff --git a/sim/AtmosphericModel.h b/sim/AtmosphericModel.h index deac8f5..0c61b14 100644 --- a/sim/AtmosphericModel.h +++ b/sim/AtmosphericModel.h @@ -13,6 +13,10 @@ public: virtual double getDensity(double altitude) = 0; virtual double getPressure(double altitude) = 0; virtual double getTemperature(double altitude) = 0; + + virtual double getSpeedOfSound(double altitude) = 0; + virtual double getDynamicViscosity(double altitude) = 0; + }; } // namespace sim diff --git a/sim/CMakeLists.txt b/sim/CMakeLists.txt index 41e05e5..b372b31 100644 --- a/sim/CMakeLists.txt +++ b/sim/CMakeLists.txt @@ -1,4 +1,6 @@ add_library(sim + Aero.cpp + Aero.h AtmosphericModel.h ConstantAtmosphere.h ConstantGravityModel.h diff --git a/sim/ConstantAtmosphere.h b/sim/ConstantAtmosphere.h index c37ad43..226eead 100644 --- a/sim/ConstantAtmosphere.h +++ b/sim/ConstantAtmosphere.h @@ -3,6 +3,7 @@ // qtrocket headers #include "AtmosphericModel.h" +#include "utils/math/Constants.h" namespace sim { @@ -15,6 +16,10 @@ public: double getDensity(double) override { return 1.225; } double getPressure(double) override { return 101325.0; } double getTemperature(double) override { return 288.15; } + + double getSpeedOfSound(double) override { return 340.294; } + + double getDynamicViscosity(double) override { return 1.78938e-5; } }; } // namespace sim diff --git a/sim/Propagator.cpp b/sim/Propagator.cpp index 5f7a998..3b322d2 100644 --- a/sim/Propagator.cpp +++ b/sim/Propagator.cpp @@ -22,10 +22,10 @@ namespace sim { -Propagator::Propagator(std::shared_ptr r) +Propagator::Propagator(std::shared_ptr r) : linearIntegrator(), //orientationIntegrator(), - rocket(r), + object(r), currentState(), nextState(), currentGravity(), @@ -154,7 +154,7 @@ void Propagator::runUntilTerminate() { states.push_back(std::make_pair(currentTime, nextState)); } - if(rocket->terminateCondition(std::make_pair(currentTime, currentState))) + if(object->terminateCondition(std::make_pair(currentTime, currentState))) break; currentTime += timeStep; @@ -171,27 +171,27 @@ void Propagator::runUntilTerminate() double Propagator::getMass() { - return rocket->getMass(currentTime); + return object->getMass(currentTime); } double Propagator::getForceX() { QtRocket* qtrocket = QtRocket::getInstance(); - return (currentState.velocity[0] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState.position[2])/ 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState.velocity[0]* currentState.velocity[0]; + return (currentState.velocity[0] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState.position[2])/ 2.0 * 0.008107 * object->getDragCoefficient() * currentState.velocity[0]* currentState.velocity[0]; } double Propagator::getForceY() { QtRocket* qtrocket = QtRocket::getInstance(); - return (currentState.velocity[1] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState.position[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState.velocity[1]* currentState.velocity[1]; + return (currentState.velocity[1] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState.position[2]) / 2.0 * 0.008107 * object->getDragCoefficient() * currentState.velocity[1]* currentState.velocity[1]; } double Propagator::getForceZ() { QtRocket* qtrocket = QtRocket::getInstance(); double gravity = (qtrocket->getEnvironment()->getGravityModel()->getAccel(currentState.position[0], currentState.position[1], currentState.position[2]))[2]; - double airDrag = (currentState.velocity[2] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState.position[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState.velocity[2]* currentState.velocity[2]; - double thrust = rocket->getThrust(currentTime); + double airDrag = (currentState.velocity[2] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState.position[2]) / 2.0 * 0.008107 * object->getDragCoefficient() * currentState.velocity[2]* currentState.velocity[2]; + double thrust = object->getThrust(currentTime); return gravity + airDrag + thrust; } diff --git a/sim/Propagator.h b/sim/Propagator.h index ec7fbe4..37568c6 100644 --- a/sim/Propagator.h +++ b/sim/Propagator.h @@ -14,10 +14,14 @@ #include "sim/RK4Solver.h" #include "utils/math/MathTypes.h" #include "sim/StateData.h" +#include "model/Propagatable.h" // Forward declare +namespace model +{ class Rocket; +} class QtRocket; namespace sim @@ -26,7 +30,7 @@ namespace sim class Propagator { public: - Propagator(std::shared_ptr r); + Propagator(std::shared_ptr r); ~Propagator(); void setInitialState(const StateData& initialState) @@ -70,7 +74,7 @@ private: std::unique_ptr> linearIntegrator; // std::unique_ptr> orientationIntegrator; - std::shared_ptr rocket; + std::shared_ptr object; StateData currentState; StateData nextState; diff --git a/sim/StateData.h b/sim/StateData.h index 43c820f..c7a268b 100644 --- a/sim/StateData.h +++ b/sim/StateData.h @@ -74,12 +74,11 @@ public: //} // private: - // These are 4-vectors so quaternion multiplication works out. The last term (scalar) is always - // zero. I could just use quaternions here, but I want to make it clear that these aren't - // rotations, they're vectors + // Intended to be used as world state data Vector3 position{0.0, 0.0, 0.0}; Vector3 velocity{0.0, 0.0, 0.0}; + // Orientation of body coordinates w.r.t. world coordinates Quaternion orientation{0.0, 0.0, 0.0, 0.0}; /// (vector, scalar) Quaternion orientationRate{0.0, 0.0, 0.0, 0.0}; /// (vector, scalar) diff --git a/sim/USStandardAtmosphere.cpp b/sim/USStandardAtmosphere.cpp index 54231ba..1081b00 100644 --- a/sim/USStandardAtmosphere.cpp +++ b/sim/USStandardAtmosphere.cpp @@ -80,8 +80,6 @@ utils::BinMap USStandardAtmosphere::standardTemperature(initTemps()); utils::BinMap USStandardAtmosphere::standardDensity(initDensities()); utils::BinMap USStandardAtmosphere::standardPressure(initPressures()); - - USStandardAtmosphere::USStandardAtmosphere() { @@ -119,7 +117,6 @@ double USStandardAtmosphere::getTemperature(double altitude) double baseAltitude = standardTemperature.getBinBase(altitude); return baseTemp - (altitude - baseAltitude) * temperatureLapseRate[altitude]; - return 0.0; } double USStandardAtmosphere::getPressure(double altitude) { @@ -142,4 +139,17 @@ double USStandardAtmosphere::getPressure(double altitude) } } + +double USStandardAtmosphere::getSpeedOfSound(double altitude) +{ + return std::sqrt( (Constants::gamma * Constants::Rstar * getTemperature(altitude)) + / + Constants::airMolarMass); +} + +double USStandardAtmosphere::getDynamicViscosity(double altitude) +{ + double temperature = getTemperature(altitude); + return (Constants::beta * std::pow(temperature, 1.5)) / ( temperature + Constants::S); +} } // namespace sim diff --git a/sim/USStandardAtmosphere.h b/sim/USStandardAtmosphere.h index 4b18083..740df66 100644 --- a/sim/USStandardAtmosphere.h +++ b/sim/USStandardAtmosphere.h @@ -28,6 +28,10 @@ public: double getPressure(double altitude) override; double getTemperature(double altitude) override; + double getSpeedOfSound(double altitude) override; + + double getDynamicViscosity(double altitude) override; + private: static utils::BinMap temperatureLapseRate; static utils::BinMap standardTemperature; diff --git a/sim/tests/USStandardAtmosphereTests.cpp b/sim/tests/USStandardAtmosphereTests.cpp index a1375a4..cafe226 100644 --- a/sim/tests/USStandardAtmosphereTests.cpp +++ b/sim/tests/USStandardAtmosphereTests.cpp @@ -6,26 +6,85 @@ TEST(USStandardAtmosphereTests, DensityTests) { sim::USStandardAtmosphere atmosphere; - // Test that the calucated values are with 1% of the published values in the NOAA report - EXPECT_NEAR(atmosphere.getDensity(0.0) / 1.225, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(1000.0) / 1.112, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(2000.0) / 1.007, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(3000.0) / 0.9093, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(4000.0) / 0.8194, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(5000.0) / 0.7364, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(6000.0) / 0.6601, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(7000.0) / 0.5900, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(8000.0) / 0.5258, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(9000.0) / 0.4647, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(10000.0) / 0.4135, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(15000.0) / 0.1948, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(20000.0) / 0.08891, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(25000.0) / 0.039466, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(30000.0) / 0.018012, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(40000.0) / 0.0038510, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(50000.0) / 0.00097752, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(60000.0) / 0.00028832, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(70000.0) / 0.000074243, 1.0, 0.01); - EXPECT_NEAR(atmosphere.getDensity(80000.0) / 0.000015701, 1.0, 0.01); + // Test that the calucated values are with 0.1% of the published values in the NOAA report + EXPECT_NEAR(atmosphere.getDensity(0.0) / 1.225, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(1000.0) / 1.112, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(2000.0) / 1.007, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(3000.0) / 0.9093, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(4000.0) / 0.8194, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(5000.0) / 0.7364, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(6000.0) / 0.6601, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(7000.0) / 0.5900, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(15000.0) / 0.19367, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(20000.0) / 0.088035, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(25000.0) / 0.039466, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(30000.0) / 0.018012, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getDensity(40000.0) / 0.0038510, 1.0, 0.001); + + // These are generally accurate to ~0.5%. Slight deviation of calculated + // density from the given density in the report table + EXPECT_NEAR(atmosphere.getDensity(8000.0) / 0.52579, 1.0, 0.005); + EXPECT_NEAR(atmosphere.getDensity(9000.0) / 0.46706, 1.0, 0.005); + EXPECT_NEAR(atmosphere.getDensity(10000.0) / 0.41351, 1.0, 0.005); + EXPECT_NEAR(atmosphere.getDensity(50000.0) / 0.00097752, 1.0, 0.005); + EXPECT_NEAR(atmosphere.getDensity(60000.0) / 0.00028832, 1.0, 0.005); + EXPECT_NEAR(atmosphere.getDensity(70000.0) / 0.000074243, 1.0, 0.005); + EXPECT_NEAR(atmosphere.getDensity(80000.0) / 0.000015701, 1.0, 0.005); } + +TEST(USStandardAtmosphereTests, PressureTests) +{ + sim::USStandardAtmosphere atmosphere; + + // Test that the calucated values are with 0.1% of the published values in the NOAA report + EXPECT_NEAR(atmosphere.getPressure(0.0) / 101325.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(1000.0) / 89876.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(2000.0) / 79501.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(3000.0) / 70108.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(4000.0) / 61640.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(5000.0) / 54019.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(6000.0) / 47181.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(7000.0) / 41060.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(8000.0) / 35599.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(9000.0) / 30742.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(10000.0) / 26436.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(15000.0) / 12044.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(20000.0) / 5474.8, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(25000.0) / 2511.0, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(30000.0) / 1171.8, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(40000.0) / 277.52, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(50000.0) / 75.944, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(60000.0) / 20.314, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(70000.0) / 4.6342, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getPressure(80000.0) / 0.88627, 1.0, 0.001); + +} + +TEST(USStandardAtmosphereTests, TemperatureTests) +{ + sim::USStandardAtmosphere atmosphere; + + // Test that the calucated values are with 0.1% of the published values in the NOAA report + EXPECT_NEAR(atmosphere.getTemperature(0.0) / 288.15, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(1000.0) / 281.651, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(2000.0) / 275.154, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(3000.0) / 268.659, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(4000.0) / 262.166, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(5000.0) / 255.676, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(6000.0) / 249.187, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(7000.0) / 242.7, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(8000.0) / 236.215, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(9000.0) / 229.733, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(10000.0) / 223.252, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(15000.0) / 216.65, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(20000.0) / 216.65, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(25000.0) / 221.552, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(30000.0) / 226.509, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(40000.0) / 251.05, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(50000.0) / 270.65, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(60000.0) / 245.45, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(70000.0) / 217.450, 1.0, 0.001); + EXPECT_NEAR(atmosphere.getTemperature(80000.0) / 196.650, 1.0, 0.001); + +} \ No newline at end of file diff --git a/utils/math/Constants.h b/utils/math/Constants.h index 13c0f18..cfc1bbe 100644 --- a/utils/math/Constants.h +++ b/utils/math/Constants.h @@ -6,9 +6,19 @@ namespace utils::math namespace Constants { - constexpr double Rstar = 8.3144598; + constexpr double Rstar = 8.31432; constexpr const double g0 = 9.80665; constexpr const double airMolarMass = 0.0289644; + + // gamma is the ratio of the specific heat of air at constant pressure to that at + // constant volume. Used in computing the speed of sound + constexpr const double gamma = 1.4; + + // beta is used in calculating the dynamic viscosity of air. Based on the 1976 US Standard + // Atmosphere report, it is empirically measured to be: + constexpr const double beta = 1.458e-6; + // Sutherland's constant + constexpr const double S = 110.4; constexpr const double standardTemperature = 288.15; constexpr const double standardDensity = 1.2250; constexpr const double meanEarthRadiusWGS84 = 6371008.8;