diff --git a/QtRocket.h b/QtRocket.h index eaf2381..7852928 100644 --- a/QtRocket.h +++ b/QtRocket.h @@ -59,13 +59,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.second->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 std::vector& initState) { rocket.second->setInitialState(initState); } + void setInitialState(const StateData& initState) { rocket.second->setInitialState(initState); } private: QtRocket(); diff --git a/gui/AnalysisWindow.cpp b/gui/AnalysisWindow.cpp index b8096f8..de60f10 100644 --- a/gui/AnalysisWindow.cpp +++ b/gui/AnalysisWindow.cpp @@ -36,7 +36,7 @@ AnalysisWindow::~AnalysisWindow() void AnalysisWindow::onButton_plotAltitude_clicked() { QtRocket* qtRocket = QtRocket::getInstance(); - const std::vector>& res = qtRocket->getStates(); + const std::vector>& res = qtRocket->getStates(); auto& plot = ui->plotWidget; plot->clearGraphs(); plot->setInteraction(QCP::iRangeDrag, true); @@ -46,7 +46,7 @@ void AnalysisWindow::onButton_plotAltitude_clicked() for (int i = 0; i < tData.size(); ++i) { tData[i] = res[i].first; - zData[i] = res[i].second[2]; + zData[i] = res[i].second.position[2]; } // create graph and assign data to it: plot->addGraph(); @@ -63,7 +63,7 @@ void AnalysisWindow::onButton_plotAltitude_clicked() void AnalysisWindow::onButton_plotVelocity_clicked() { QtRocket* qtRocket = QtRocket::getInstance(); - const std::vector>& res = qtRocket->getStates(); + const std::vector>& res = qtRocket->getStates(); auto& plot = ui->plotWidget; plot->clearGraphs(); plot->setInteraction(QCP::iRangeDrag, true); @@ -74,7 +74,7 @@ void AnalysisWindow::onButton_plotVelocity_clicked() for (int i = 0; i < tData.size(); ++i) { tData[i] = res[i].first; - zData[i] = res[i].second[5]; + zData[i] = res[i].second.velocity[2]; } // create graph and assign data to it: plot->addGraph(); diff --git a/gui/MainWindow.cpp b/gui/MainWindow.cpp index 054c266..c2ab9c0 100644 --- a/gui/MainWindow.cpp +++ b/gui/MainWindow.cpp @@ -123,7 +123,10 @@ void MainWindow::onButton_calculateTrajectory_clicked() double initialVelocityX = initialVelocity * std::cos(initialAngle / 57.2958); double initialVelocityZ = initialVelocity * std::sin(initialAngle / 57.2958); - std::vector initialState = {0.0, 0.0, 0.0, initialVelocityX, 0.0, initialVelocityZ, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + //std::vector initialState = {0.0, 0.0, 0.0, initialVelocityX, 0.0, initialVelocityZ, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + StateData initialState; + initialState.position = {0.0, 0.0, 0.0}; + initialState.velocity = {initialVelocityX, 0.0, initialVelocityZ}; auto rocket = QtRocket::getInstance()->getRocket(); rocket->setMass(mass); rocket->setDragCoefficient(dragCoeff); diff --git a/model/Rocket.cpp b/model/Rocket.cpp index 7bf0fa5..350dd65 100644 --- a/model/Rocket.cpp +++ b/model/Rocket.cpp @@ -18,10 +18,10 @@ void Rocket::setMotorModel(const model::MotorModel& motor) mm = motor; } -bool Rocket::terminateCondition(const std::pair& cond) +bool Rocket::terminateCondition(const std::pair& cond) { // Terminate propagation when the z coordinate drops below zero - if(cond.second[2] < 0) + if(cond.second.position[2] < 0) return true; else return false; diff --git a/model/Rocket.h b/model/Rocket.h index 2823843..d7c874f 100644 --- a/model/Rocket.h +++ b/model/Rocket.h @@ -87,7 +87,7 @@ public: * @param cond time/state pair * @return true if the passed-in time/state satisfies the terminate condition */ - bool terminateCondition(const std::pair& cond); + bool terminateCondition(const std::pair& cond); /** * @brief setName sets the rocket name diff --git a/sim/Propagator.cpp b/sim/Propagator.cpp index b33a15d..5f7a998 100644 --- a/sim/Propagator.cpp +++ b/sim/Propagator.cpp @@ -24,10 +24,16 @@ namespace sim { Propagator::Propagator(std::shared_ptr r) : linearIntegrator(), - orientationIntegrator(), + //orientationIntegrator(), rocket(r), - currentBodyState(), - worldFrameState() + currentState(), + nextState(), + currentGravity(), + currentWindSpeed(), + saveStates(true), + currentTime(0.0), + timeStep(0.01), + states() { @@ -113,16 +119,16 @@ Propagator::Propagator(std::shared_ptr r) // })); // orientationIntegrator->setTimeStep(timeStep); - std::function(Quaternion&, Quaternion&)> orientationODE = - [this](Quaternion& qOri, Quaternion& qRate) -> std::pair - { - Quaternion dOri; - Quaternion dOriRate; +// std::function(Quaternion&, Quaternion&)> orientationODE = +// [this](Quaternion& qOri, Quaternion& qRate) -> std::pair +// { +// Quaternion dOri; +// Quaternion dOriRate; - Matrix4 - } +// Matrix4 +// } - saveStates = true; + saveStates = true; } Propagator::~Propagator() @@ -134,27 +140,25 @@ void Propagator::runUntilTerminate() 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) { - // Reset the body frame positions to zero since the origin of the body frame is the CM - currentBodyState[0] = 0.0; - currentBodyState[1] = 0.0; - currentBodyState[2] = 0.0; - // tempRes gets overwritten - tempRes = linearIntegrator->step(currentBodyState); + std::tie(nextState.position, nextState.velocity) = linearIntegrator->step(currentState.position, currentState.velocity); + + //tempRes = linearIntegrator->step(currentBodyState); - std::swap(currentBodyState, tempRes); if(saveStates) { - states.push_back(std::make_pair(currentTime, currentBodyState)); + states.push_back(std::make_pair(currentTime, nextState)); } - if(rocket->terminateCondition(std::make_pair(currentTime, currentBodyState))) + if(rocket->terminateCondition(std::make_pair(currentTime, currentState))) break; currentTime += timeStep; + currentState = nextState; } endTime = std::chrono::steady_clock::now(); @@ -173,20 +177,20 @@ double Propagator::getMass() double Propagator::getForceX() { QtRocket* qtrocket = QtRocket::getInstance(); - return (currentBodyState[3] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentBodyState[2])/ 2.0 * 0.008107 * rocket->getDragCoefficient() * currentBodyState[3]* currentBodyState[3]; + 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]; } double Propagator::getForceY() { QtRocket* qtrocket = QtRocket::getInstance(); - return (currentBodyState[4] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentBodyState[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentBodyState[4]* currentBodyState[4]; + 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]; } double Propagator::getForceZ() { QtRocket* qtrocket = QtRocket::getInstance(); - double gravity = (qtrocket->getEnvironment()->getGravityModel()->getAccel(currentBodyState[0], currentBodyState[1], currentBodyState[2]))[2]; - double airDrag = (currentBodyState[5] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentBodyState[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentBodyState[5]* currentBodyState[5]; + 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); return gravity + airDrag + thrust; } @@ -198,18 +202,15 @@ double Propagator::getTorqueR() { return 0.0; } Vector3 Propagator::getCurrentGravity() { auto gravityModel = QtRocket::getInstance()->getEnvironment()->getGravityModel(); - auto gravityAccel = gravityModel->getAccel(currentBodyState[0], - currentBodyState[1], - currentBodyState[2]); + auto gravityAccel = gravityModel->getAccel(currentState.position[0], + currentState.position[1], + currentState.position[2]); Vector3 gravityVector{gravityAccel[0], gravityAccel[1], gravityAccel[2]}; - Quaternion q{currentOrientation[3], - currentOrientation[4], - currentOrientation[5], - currentOrientation[6]}; + Quaternion q = currentState.orientation; Vector3 res = q * gravityVector; diff --git a/sim/Propagator.h b/sim/Propagator.h index ada9686..2926101 100644 --- a/sim/Propagator.h +++ b/sim/Propagator.h @@ -29,18 +29,14 @@ public: Propagator(std::shared_ptr r); ~Propagator(); - void setInitialState(const std::vector& initialState) + void setInitialState(const StateData& initialState) { - for(std::size_t i = 0; i < initialState.size(); ++i) - { - currentBodyState[i] = initialState[i]; - } - + currentState = initialState; } - const Vector6& getCurrentState() const + const StateData& getCurrentState() const { - return currentBodyState; + return currentState; } void runUntilTerminate(); @@ -50,7 +46,7 @@ public: saveStates = s; } - const std::vector>& getStates() const { return states; } + const std::vector>& getStates() const { return states; } void clearStates() { states.clear(); } void setCurrentTime(double t) { currentTime = t; } @@ -76,28 +72,16 @@ private: std::shared_ptr rocket; - StateData worldFrameState; - //StateData bodyFrameState; - Vector3 currentBodyPosition{0.0, 0.0, 0.0}; - Vector3 currentBodyVelocity{0.0, 0.0, 0.0}; - Vector3 nextBodyPosition{0.0, 0.0, 0.0}; - Vector3 nextBodyVelocity{0.0, 0.0, 0.0}; - - std::vector currentWorldState{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + StateData currentState; + StateData nextState; Vector3 currentGravity{0.0, 0.0, 0.0}; Vector3 currentWindSpeed{0.0, 0.0, 0.0}; - - // orientation vectors in the form (yawDot, pitchDot, rollDot, q1, q2, q3, q4) - Quaternion currentOrientation{0.0, 0.0, 0.0, 0.0}; - Quaternion currentOrientationRate{0.0, 0.0, 0.0, 0.0}; - Quaternion nextOrientation{0.0, 0.0, 0.0, 0.0}; - Quaternion nextOrientationRate{0.0, 0.0, 0.0, 0.0}; bool saveStates{true}; double currentTime{0.0}; double timeStep{0.01}; - std::vector> states; + std::vector> states; Vector3 getCurrentGravity(); }; diff --git a/sim/StateData.h b/sim/StateData.h index f20aede..43c820f 100644 --- a/sim/StateData.h +++ b/sim/StateData.h @@ -22,16 +22,56 @@ public: StateData() {} ~StateData() {} - std::vector getPosStdVector() + StateData(const StateData&) = default; + StateData(StateData&&) = default; + + StateData& operator=(const StateData& rhs) + { + if(this != &rhs) + { + position = rhs.position; + velocity = rhs.velocity; + orientation = rhs.orientation; + orientationRate = rhs.orientationRate; + dcm = rhs.dcm; + eulerAngles = rhs.eulerAngles; + } + return *this; + } + StateData& operator=(StateData&& rhs) + { + if(this != &rhs) + { + position = std::move(rhs.position); + velocity = std::move(rhs.velocity); + orientation = std::move(rhs.orientation); + orientationRate = std::move(rhs.orientationRate); + dcm = std::move(rhs.dcm); + eulerAngles = std::move(rhs.eulerAngles); + } + return *this; + } + + std::vector getPosStdVector() const { return std::vector{position[0], position[1], position[2]}; } - std::vector getVelStdVector() + std::vector getVelStdVector() const { return std::vector{velocity[0], velocity[1], velocity[2]}; } + /// TODO: Put these behind an interface + //Vector3 getPosition() const + //{ + // return position; + //} + + //Vector3 getVelocity() const + //{ + // return velocity; + //} // private: // These are 4-vectors so quaternion multiplication works out. The last term (scalar) is always @@ -51,9 +91,6 @@ public: /// roll - phi Vector3 eulerAngles{0.0, 0.0, 0.0}; - // This is an array because the integrator expects it - double data[6]; - }; #endif // STATEDATA_H diff --git a/utils/ThrustCurveAPI.cpp b/utils/ThrustCurveAPI.cpp index bd4c8c5..04b7a01 100644 --- a/utils/ThrustCurveAPI.cpp +++ b/utils/ThrustCurveAPI.cpp @@ -235,7 +235,9 @@ std::vector ThrustCurveAPI::searchMotors(const SearchCriteria { Json::Reader reader; Json::Value jsonResult; +Logger::getInstance()->debug("1"); reader.parse(result, jsonResult); +Logger::getInstance()->debug("2"); for(Json::ValueConstIterator iter = jsonResult["results"].begin(); iter != jsonResult["results"].end(); @@ -244,6 +246,7 @@ std::vector ThrustCurveAPI::searchMotors(const SearchCriteria model::MotorModel motorModel; model::MotorModel::MetaData mm; mm.commonName = (*iter)["commonName"].asString(); +Logger::getInstance()->debug("3"); std::string availability = (*iter)["availability"].asString(); if(availability == "regular") @@ -253,6 +256,7 @@ std::vector ThrustCurveAPI::searchMotors(const SearchCriteria mm.avgThrust = (*iter)["avgThrustN"].asDouble(); mm.burnTime = (*iter)["burnTimeS"].asDouble(); +Logger::getInstance()->debug("4"); // TODO fill in certOrg // TODO fill in delays mm.designation = (*iter)["designation"].asString(); @@ -279,8 +283,10 @@ std::vector ThrustCurveAPI::searchMotors(const SearchCriteria else mm.type = model::MotorModel::MotorType(model::MotorModel::MOTORTYPE::HYBRID); - motorModel.moveMetaData(std::move(mm)); +Logger::getInstance()->debug("5"); auto tc = getThrustCurve(mm.motorIdTC); + motorModel.moveMetaData(std::move(mm)); +Logger::getInstance()->debug("6"); if(tc) { motorModel.addThrustCurve(*tc);