Building and running again

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<std::pair<double, Vector6>>& getStates() const { return rocket.second->getStates(); }
+   const std::vector<std::pair<double, StateData>>& 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<double>& initState) { rocket.second->setInitialState(initState); }
+   void setInitialState(const StateData& initState) { rocket.second->setInitialState(initState); }
 
 private:
    QtRocket();

gui/AnalysisWindow.cpp

@@ -36,7 +36,7 @@ AnalysisWindow::~AnalysisWindow()
 void AnalysisWindow::onButton_plotAltitude_clicked()
 {
    QtRocket* qtRocket = QtRocket::getInstance();
-   const std::vector<std::pair<double, Vector6>>& res = qtRocket->getStates();
+   const std::vector<std::pair<double, StateData>>& 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<std::pair<double, Vector6>>& res = qtRocket->getStates();
+   const std::vector<std::pair<double, StateData>>& 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();

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<double> 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<double> 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);

model/Rocket.cpp

@@ -18,10 +18,10 @@ void Rocket::setMotorModel(const model::MotorModel& motor)
    mm = motor;
 }
 
-bool Rocket::terminateCondition(const std::pair<double, Vector6>& cond)
+bool Rocket::terminateCondition(const std::pair<double, StateData>& 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;

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<double, Vector6>& cond);
+   bool terminateCondition(const std::pair<double, StateData>& cond);
 
    /**
     * @brief setName sets the rocket name

sim/Propagator.cpp

@@ -24,10 +24,16 @@ namespace sim {
 
 Propagator::Propagator(std::shared_ptr<Rocket> r)
    : linearIntegrator(),
-     orientationIntegrator(),
+     //orientationIntegrator(),
      rocket(r),
-     currentBodyState(),
+     currentState(),
-     worldFrameState()
+     nextState(),
+     currentGravity(),
+     currentWindSpeed(),
+     saveStates(true),
+     currentTime(0.0),
+     timeStep(0.01),
+     states()
 {
 
 
@@ -113,16 +119,16 @@ Propagator::Propagator(std::shared_ptr<Rocket> r)
 //       }));
 //   orientationIntegrator->setTimeStep(timeStep);
 
-   std::function<std::pair<Quaternion, Quaternion>(Quaternion&, Quaternion&)> orientationODE =
+//  std::function<std::pair<Quaternion, Quaternion>(Quaternion&, Quaternion&)> orientationODE =
-   [this](Quaternion& qOri, Quaternion& qRate) -> std::pair<Quaternion, Quaternion>
+//  [this](Quaternion& qOri, Quaternion& qRate) -> std::pair<Quaternion, Quaternion>
-   {
+//  {
-      Quaternion dOri;
+//     Quaternion dOri;
-      Quaternion dOriRate;
+//     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 gravity = (qtrocket->getEnvironment()->getGravityModel()->getAccel(currentState.position[0], currentState.position[1], currentState.position[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 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],
+   auto gravityAccel = gravityModel->getAccel(currentState.position[0],
-                                              currentBodyState[1],
+                                              currentState.position[1],
-                                              currentBodyState[2]);
+                                              currentState.position[2]);
    Vector3 gravityVector{gravityAccel[0],
                          gravityAccel[1],
                          gravityAccel[2]};
    
 
-   Quaternion q{currentOrientation[3],
+   Quaternion q = currentState.orientation;
-                currentOrientation[4],
-                currentOrientation[5],
-                currentOrientation[6]};
 
    Vector3 res = q * gravityVector;
    

sim/Propagator.h

@@ -29,18 +29,14 @@ public:
     Propagator(std::shared_ptr<Rocket> r);
     ~Propagator();
 
-    void setInitialState(const std::vector<double>& initialState)
+    void setInitialState(const StateData& initialState)
     {
-       for(std::size_t i = 0; i < initialState.size(); ++i)
+           currentState = initialState;
-       {
-           currentBodyState[i] = initialState[i];
-       }
-
     }
 
-    const Vector6& getCurrentState() const
+    const StateData& getCurrentState() const
     {
-       return currentBodyState;
+       return currentState;
     }
 
     void runUntilTerminate();
@@ -50,7 +46,7 @@ public:
        saveStates = s;
     }
 
-    const std::vector<std::pair<double, Vector6>>& getStates() const { return states; }
+    const std::vector<std::pair<double, StateData>>& getStates() const { return states; }
 
     void clearStates() { states.clear(); }
     void setCurrentTime(double t) { currentTime = t; }
@@ -76,28 +72,16 @@ private:
 
    std::shared_ptr<Rocket> rocket;
 
-   StateData worldFrameState;
+   StateData currentState;
-   //StateData bodyFrameState;
+   StateData nextState;
-   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<double> currentWorldState{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 
    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<std::pair<double, Vector6>> states;
+   std::vector<std::pair<double, StateData>> states;
 
    Vector3 getCurrentGravity();
 };

sim/StateData.h

@@ -22,16 +22,56 @@ public:
    StateData() {}
    ~StateData() {}
 
-   std::vector<double> 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<double> getPosStdVector() const
    {
       return std::vector<double>{position[0], position[1], position[2]};
    }
-   std::vector<double> getVelStdVector()
+   std::vector<double> getVelStdVector() const
    {
       return std::vector<double>{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

utils/ThrustCurveAPI.cpp

@@ -235,7 +235,9 @@ std::vector<model::MotorModel> 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<model::MotorModel> 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<model::MotorModel> 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<model::MotorModel> 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);