From 5a332ec0606f31eefe2bf5f45644f5487e7f3eba Mon Sep 17 00:00:00 2001 From: Travis Hunter Date: Sun, 17 Mar 2024 10:19:39 -0600 Subject: [PATCH] Substantial refactor and cleanup of Propagator and Rocket class (now RocketModel class) --- QtRocket.cpp | 9 +- QtRocket.h | 17 +- gui/AnalysisWindow.cpp | 2 +- gui/MainWindow.cpp | 2 +- model/CMakeLists.txt | 4 +- model/Propagatable.h | 18 +- model/{Rocket.cpp => RocketModel.cpp} | 26 +-- model/{Rocket.h => RocketModel.h} | 12 +- sim/Propagator.cpp | 232 ++++++-------------------- sim/Propagator.h | 30 +--- 10 files changed, 101 insertions(+), 251 deletions(-) rename model/{Rocket.cpp => RocketModel.cpp} (64%) rename model/{Rocket.h => RocketModel.h} (93%) diff --git a/QtRocket.cpp b/QtRocket.cpp index 764a057..b544ed9 100644 --- a/QtRocket.cpp +++ b/QtRocket.cpp @@ -80,7 +80,7 @@ QtRocket::QtRocket() setEnvironment(std::make_shared()); rocket.first = - std::make_shared(); + std::make_shared(); rocket.second = std::make_shared(rocket.first); @@ -113,7 +113,7 @@ int QtRocket::run(int argc, char* argv[]) void QtRocket::launchRocket() { // initialize the propagator - rocket.second->clearStates(); + rocket.first->clearStates(); rocket.second->setCurrentTime(0.0); // start the rocket motor @@ -128,8 +128,3 @@ void QtRocket::addMotorModels(std::vector& m) motorDatabase->addMotorModels(m); // TODO: Now clear any duplicates? } - -void QtRocket::appendState(const StateData& state) -{ - states.emplace_back(state); -} diff --git a/QtRocket.h b/QtRocket.h index 613bfd3..a9efd7b 100644 --- a/QtRocket.h +++ b/QtRocket.h @@ -14,7 +14,7 @@ // qtrocket headers #include "model/MotorModel.h" -#include "model/Rocket.h" +#include "model/RocketModel.h" #include "sim/Environment.h" #include "sim/Propagator.h" #include "utils/Logger.h" @@ -43,13 +43,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; rocket.second = std::make_shared(r); } void setEnvironment(std::shared_ptr e) { environment = e; } @@ -58,15 +58,13 @@ public: * @brief getStates returns a vector of time/state pairs generated during launch() * @return vector of pairs of doubles, where the first value is a time and the second a state vector */ - const std::vector>& getStates() const { return rocket.second->getStates(); } + const std::vector>& getStates() const { return rocket.first->getStates(); } /** * @brief setInitialState sets the initial state of the Rocket. * @param initState initial state vector (x, y, z, xDot, yDot, zDot, pitch, yaw, roll, pitchDot, yawDot, rollDot) */ - void setInitialState(const StateData& initState) { rocket.second->setInitialState(initState); } - - void appendState(const StateData& state); + void setInitialState(const StateData& initState) { rocket.first->setInitialState(initState); } private: QtRocket(); @@ -80,7 +78,8 @@ private: utils::Logger* logger; - std::pair, std::shared_ptr> rocket; + using Rocket = std::pair, std::shared_ptr>; + Rocket rocket; std::shared_ptr environment; std::shared_ptr motorDatabase; @@ -92,8 +91,6 @@ private: // Table of state data std::vector states; - - }; #endif // QTROCKET_H diff --git a/gui/AnalysisWindow.cpp b/gui/AnalysisWindow.cpp index 65d5d6f..7d0e783 100644 --- a/gui/AnalysisWindow.cpp +++ b/gui/AnalysisWindow.cpp @@ -91,7 +91,7 @@ void AnalysisWindow::onButton_plotVelocity_clicked() void AnalysisWindow::onButton_plotMotorCurve_clicked() { - std::shared_ptr rocket = QtRocket::getInstance()->getRocket(); + std::shared_ptr rocket = QtRocket::getInstance()->getRocket(); model::MotorModel motor = rocket->getMotorModel(); ThrustCurve tc = motor.getThrustCurve(); diff --git a/gui/MainWindow.cpp b/gui/MainWindow.cpp index c2ab9c0..12ec10a 100644 --- a/gui/MainWindow.cpp +++ b/gui/MainWindow.cpp @@ -20,7 +20,7 @@ #include "gui/MainWindow.h" #include "gui/ThrustCurveMotorSelector.h" #include "gui/SimOptionsWindow.h" -#include "model/Rocket.h" +#include "model/RocketModel.h" #include "utils/RSEDatabaseLoader.h" diff --git a/model/CMakeLists.txt b/model/CMakeLists.txt index bf9be41..afa0fd1 100644 --- a/model/CMakeLists.txt +++ b/model/CMakeLists.txt @@ -7,8 +7,8 @@ add_library(model Part.h Propagatable.cpp Propagatable.h - Rocket.cpp - Rocket.h + RocketModel.cpp + RocketModel.h ThrustCurve.cpp ThrustCurve.h InertiaTensors.h) diff --git a/model/Propagatable.h b/model/Propagatable.h index 02e50c8..d8a8a6e 100644 --- a/model/Propagatable.h +++ b/model/Propagatable.h @@ -30,11 +30,27 @@ public: virtual bool terminateCondition(double t) = 0; + void setCurrentState(const StateData& st) { currentState = st; } + const StateData& getCurrentState() { return currentState; } + + const StateData& getInitialState() { return initialState; } + void setInitialState(const StateData& init) { initialState = init; } + + void appendState(double t, const StateData& st) { states.emplace_back(t, st); } + + const std::vector>& getStates() { return states; } + + void clearStates() { states.clear(); } + protected: sim::Aero aeroData; - StateData state; + StateData initialState; + StateData currentState; + StateData nextState; + + std::vector> states; }; } diff --git a/model/Rocket.cpp b/model/RocketModel.cpp similarity index 64% rename from model/Rocket.cpp rename to model/RocketModel.cpp index 032be0b..145f8a3 100644 --- a/model/Rocket.cpp +++ b/model/RocketModel.cpp @@ -1,41 +1,41 @@ // qtrocket headers -#include "Rocket.h" +#include "RocketModel.h" #include "QtRocket.h" #include "InertiaTensors.h" namespace model -{ +{ -Rocket::Rocket() +RocketModel::RocketModel() : topPart("NoseCone", InertiaTensors::SolidSphere(1.0), 1.0, {0.0, 0.0, 1.0}) { } -double Rocket::getMass(double t) +double RocketModel::getMass(double t) { double mass = mm.getMass(t); mass += topPart.getCompositeMass(t); return mass; } -Matrix3 Rocket::getInertiaTensor(double) +Matrix3 RocketModel::getInertiaTensor(double) { return topPart.getCompositeI(); } -bool Rocket::terminateCondition(double) +bool RocketModel::terminateCondition(double) { // Terminate propagation when the z coordinate drops below zero - if(state.position[2] < 0) + if(currentState.position[2] < 0) return true; else return false; } -Vector3 Rocket::getForces(double t) +Vector3 RocketModel::getForces(double t) { // Get thrust // Assume that thrust is always through the center of mass and in the rocket's Z-axis @@ -45,7 +45,7 @@ Vector3 Rocket::getForces(double t) // Get gravity auto gravityModel = QtRocket::getInstance()->getEnvironment()->getGravityModel(); - Vector3 gravity = gravityModel->getAccel(state.position)*getMass(t); + Vector3 gravity = gravityModel->getAccel(currentState.position)*getMass(t); forces += gravity; @@ -55,23 +55,23 @@ Vector3 Rocket::getForces(double t) return forces; } -Vector3 Rocket::getTorques(double t) +Vector3 RocketModel::getTorques(double t) { return Vector3{0.0, 0.0, 0.0}; } -double Rocket::getThrust(double t) +double RocketModel::getThrust(double t) { return mm.getThrust(t); } -void Rocket::launch() +void RocketModel::launch() { mm.startMotor(0.0); } -void Rocket::setMotorModel(const model::MotorModel& motor) +void RocketModel::setMotorModel(const model::MotorModel& motor) { mm = motor; } diff --git a/model/Rocket.h b/model/RocketModel.h similarity index 93% rename from model/Rocket.h rename to model/RocketModel.h index 22a2fb2..c6be016 100644 --- a/model/Rocket.h +++ b/model/RocketModel.h @@ -1,5 +1,5 @@ -#ifndef ROCKET_H -#define ROCKET_H +#ifndef ROCKETMODEL_H +#define ROCKETMODEL_H /// \cond // C headers @@ -28,19 +28,19 @@ namespace model * @brief The Rocket class holds all rocket components * */ -class Rocket : public Propagatable +class RocketModel : public Propagatable { public: /** * @brief Rocket class constructor */ - Rocket(); + RocketModel(); /** * @brief Rocket class destructor * */ - virtual ~Rocket() {} + virtual ~RocketModel() {} /** * @brief launch Propagates the Rocket object until termination, @@ -113,4 +113,4 @@ private: }; } // namespace model -#endif // ROCKET_H +#endif // ROCKETMODEL_H diff --git a/sim/Propagator.cpp b/sim/Propagator.cpp index 699d61b..2337a9e 100644 --- a/sim/Propagator.cpp +++ b/sim/Propagator.cpp @@ -13,121 +13,36 @@ // qtrocket headers #include "Propagator.h" -#include "QtRocket.h" -#include "model/Rocket.h" #include "sim/RK4Solver.h" #include "utils/Logger.h" - -namespace sim { - -Propagator::Propagator(std::shared_ptr r) - : linearIntegrator(), - //orientationIntegrator(), - object(r), - currentState(), - nextState(), - currentGravity(), - currentWindSpeed(), - saveStates(true), - currentTime(0.0), - timeStep(0.01), - states() +namespace sim { +Propagator::Propagator(std::shared_ptr r) + : linearIntegrator(), + object(r), + saveStates(true), + timeStep(0.01) +{ + // Linear velocity and acceleration + std::function(Vector3&, Vector3&)> linearODEs = [this](Vector3& state, Vector3& rate) -> std::pair + { + Vector3 dPosition; + Vector3 dVelocity; + // dx/dt + dPosition = rate; - // This is a little strange, but I have to populate the integrator unique_ptr - // with reset. make_unique() doesn't work because the compiler can't seem to - // deduce the template parameters correctly, and I don't want to specify them - // manually either. RK4Solver constructor is perfectly capable of deducing it's - // template types, and it derives from DESolver, so we can just reset the unique_ptr - // and pass it a freshly allocated RK4Solver pointer + // dvx/dt + dVelocity = object->getForces(currentTime) / object->getMass(currentTime); - // The state vector has components of the form: + return std::make_pair(dPosition, dVelocity); + }; - // Linear velocity and acceleration - std::function(Vector3&, Vector3&)> linearODE = [this](Vector3& state, Vector3& rate) -> std::pair - { - Vector3 dPosition; - Vector3 dVelocity; - // dx/dt - dPosition[0] = rate[0]; + linearIntegrator.reset(new RK4Solver(linearODEs)); + linearIntegrator->setTimeStep(timeStep); - // dy/dt - dPosition[1] = rate[1]; - - // dz/dt - dPosition[2] = rate[2]; - - // dvx/dt - dVelocity[0] = getForceX() / getMass(); - - // dvy/dt - dVelocity[1] = getForceY() / getMass(); - - // dvz/dt - dVelocity[2] = getForceZ() / getMass(); - - return std::make_pair(dPosition, dVelocity); - - }; - - linearIntegrator.reset(new RK4Solver(linearODE)); - linearIntegrator->setTimeStep(timeStep); - - // This is pure quaternion - // This formula is taken from https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-attitudetran - // - // Convention for the quaternions is (vector, scalar). So q4 is scalar term - // - // q1Dot = 1/2 * [(q4*omega1) - (q3*omega2) + (q2*omega3)] - // q2Dot = 1/2 * [(q3*omega1) + (q4*omega2) - (q1*omega3)] - // q3Dot = 1/2 * [(-q2*omega1) + (q1*omega2) + (q4*omega3)] - // q4Dot = -1/2 *[(q1*omega1) + (q2*omega2) + (q3*omega3)] - // - // omega1 = yawRate - // omega2 = pitchRate - // omega3 = rollRate - // -// orientationIntegrator.reset(new RK4Solver( -// /* dyawRate/dt */ [this](const std::vector& s, double) -> double -// { return getTorqueP() / getIyaw(); }, -// /* dpitchRate/dt */ [this](const std::vector& s, double) -> double -// { return getTorqueQ() / getIpitch(); }, -// /* drollRate/dt */ [this](const std::vector& s, double) -> double -// { return getTorqueR() / getIroll(); }, -// /* q1Dot */ [](const std::vector& s, double) -> double -// { -// double retVal = s[6]*s[0] - s[5]*s[1] + s[4]*s[2]; -// return 0.5 * retVal; -// }, -// /* q2Dot */ [](const std::vector& s, double) -> double -// { -// double retVal = s[5]*s[0] + s[6]*s[1] - s[3]*s[2]; -// return 0.5 * retVal; -// }, -// /* q3Dot */ [](const std::vector& s, double) -> double -// { -// double retVal = -s[4]*s[0] + s[3]*s[1] + s[6]*s[2]; -// return 0.5 * retVal; -// }, -// /* q4Dot */ [](const std::vector& s, double) -> double -// { -// double retVal = s[3]*s[0] + s[4]*s[1] + s[5]*s[2]; -// return -0.5 * retVal; -// })); -// orientationIntegrator->setTimeStep(timeStep); - -// std::function(Quaternion&, Quaternion&)> orientationODE = -// [this](Quaternion& qOri, Quaternion& qRate) -> std::pair -// { -// Quaternion dOri; -// Quaternion dOriRate; - -// Matrix4 -// } - - saveStates = true; + saveStates = true; } Propagator::~Propagator() @@ -136,89 +51,40 @@ Propagator::~Propagator() void Propagator::runUntilTerminate() { - std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point endTime; + std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point endTime; - currentState.position = {0.0, 0.0, 0.0}; - while(true) - { + Vector3 currentPosition; + Vector3 currentVelocity; + Vector3 nextPosition; + Vector3 nextVelocity; + while(true) + { + currentPosition = object->getCurrentState().position; + currentVelocity = object->getCurrentState().velocity; - // tempRes gets overwritten - std::tie(nextState.position, nextState.velocity) = linearIntegrator->step(currentState.position, currentState.velocity); + std::tie(nextPosition, nextVelocity) = linearIntegrator->step(currentPosition, currentVelocity); - //tempRes = linearIntegrator->step(currentBodyState); + StateData nextState; + nextState.position = nextPosition; + nextState.velocity = nextVelocity; + object->setCurrentState(nextState); - - if(saveStates) - { - states.push_back(std::make_pair(currentTime, nextState)); - } - if(object->terminateCondition(currentTime)) - break; + if(saveStates) + { + object->appendState(currentTime, nextState); + } + if(object->terminateCondition(currentTime)) + break; - currentTime += timeStep; - currentState = nextState; - } - endTime = std::chrono::steady_clock::now(); - - std::stringstream duration; - duration << "runUntilTerminate time (microseconds): "; - duration << std::chrono::duration_cast(endTime - startTime).count(); - utils::Logger::getInstance()->debug(duration.str()); - -} - -double Propagator::getMass() -{ - return object->getMass(currentTime); -} - -double Propagator::getForceX() -{ - return 0.0; - //QtRocket* qtrocket = QtRocket::getInstance(); - //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() -{ - return 0.0; - //QtRocket* qtrocket = QtRocket::getInstance(); - //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() -{ - return 0.0; - //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 * object->getDragCoefficient() * currentState.velocity[2]* currentState.velocity[2]; - //double thrust = object->getThrust(currentTime); - //return gravity + airDrag + thrust; -} - -double Propagator::getTorqueP() { return 0.0; } -double Propagator::getTorqueQ() { return 0.0; } -double Propagator::getTorqueR() { return 0.0; } - -Vector3 Propagator::getCurrentGravity() -{ - auto gravityModel = QtRocket::getInstance()->getEnvironment()->getGravityModel(); - auto gravityAccel = gravityModel->getAccel(currentState.position[0], - currentState.position[1], - currentState.position[2]); - Vector3 gravityVector{gravityAccel[0], - gravityAccel[1], - gravityAccel[2]}; - - - Quaternion q = currentState.orientation; - - Vector3 res = q * gravityVector; - - - return Vector3{0.0, 0.0, 0.0}; + currentTime += timeStep; + } + endTime = std::chrono::steady_clock::now(); + std::stringstream duration; + duration << "runUntilTerminate time (microseconds): "; + duration << std::chrono::duration_cast(endTime - startTime).count(); + utils::Logger::getInstance()->debug(duration.str()); } diff --git a/sim/Propagator.h b/sim/Propagator.h index 37568c6..b9bcdcf 100644 --- a/sim/Propagator.h +++ b/sim/Propagator.h @@ -5,7 +5,6 @@ // C headers // C++ headers #include -#include // 3rd party headers /// \endcond @@ -30,17 +29,17 @@ namespace sim class Propagator { public: - Propagator(std::shared_ptr r); + Propagator(std::shared_ptr o); ~Propagator(); void setInitialState(const StateData& initialState) { - currentState = initialState; + object->setInitialState(initialState); } const StateData& getCurrentState() const { - return currentState; + return object->getCurrentState(); } void runUntilTerminate(); @@ -50,44 +49,21 @@ public: saveStates = s; } - const std::vector>& getStates() const { return states; } - - void clearStates() { states.clear(); } void setCurrentTime(double t) { currentTime = t; } void setTimeStep(double ts) { timeStep = ts; } void setSaveStats(bool s) { saveStates = s; } private: - double getMass(); - double getForceX(); - double getForceY(); - double getForceZ(); - - double getTorqueP(); // yaw - double getTorqueQ(); // pitch - double getTorqueR(); // roll - - double getIyaw() { return 1.0; } - double getIpitch() { return 1.0; } - double getIroll() { return 1.0; } std::unique_ptr> linearIntegrator; // std::unique_ptr> orientationIntegrator; std::shared_ptr object; - StateData currentState; - StateData nextState; - - Vector3 currentGravity{0.0, 0.0, 0.0}; - Vector3 currentWindSpeed{0.0, 0.0, 0.0}; - bool saveStates{true}; double currentTime{0.0}; double timeStep{0.01}; - std::vector> states; - Vector3 getCurrentGravity(); }; } // namespace sim