Fix k iteration

Propulsive-Landing · Oct 15, 2024 · 48c664d · 48c664d
1 parent a8e6546
commit 48c664d
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 6 deletions.
diff --git a/include/Controller.hpp b/include/Controller.hpp
@@ -18,6 +18,7 @@ class Controller{
     Eigen::Matrix<double, 2* MissionConstants::kNumberControllerGains, 8> controller_gains; 
     double next_tvc_time;
     double tvc_start_time;
+    double k_iteration_start_time;
     std::vector<float> controller_gain_times;
     // Eigen::Vector2d tvc_angles; [TODO MOVE TO HARDWARE]
     int current_iteration_index = 0;
@@ -40,6 +41,7 @@ class Controller{
     void Start(double current_time);
     void Center();
     void ImportControlParameters(std::string file_name);
+    void ResetKIteration(double current_time);
     Eigen::Matrix<double, 2, 8>  GetCurrentKMatrix();
 
 };
diff --git a/include/Mode.hpp b/include/Mode.hpp
@@ -31,7 +31,7 @@ class Mode
         Mode::Phase UpdateTestTVC(Navigation& navigation, Controller& controller, double currentTime);
         Mode::Phase UpdateIdle(Navigation& navigation, Controller& controller, double currentTime);
         Mode::Phase UpdateLaunch(Navigation& navigation, Controller& controller, Igniter& igniter, double current_time);
-        Mode::Phase UpdateFreefall(Navigation& navigation, Igniter& igniter, double currentTime);
+        Mode::Phase UpdateFreefall(Navigation& navigation, Controller &controller, Igniter& igniter, double currentTime);
         Mode::Phase UpdateLand(Navigation& navigation, Controller& controller, double current_time);
         Mode::Phase UpdateSafeMode(Navigation& navigation, Controller& controller, double currentTime);
 };
diff --git a/src/Controller.cpp b/src/Controller.cpp
@@ -16,6 +16,7 @@ void Controller::Start(double current_time){
 
     tvc_start_time = current_time;
     next_tvc_time = current_time;
+    ResetKIteration(current_time); // Sets iteration start time
 }
 
 
@@ -122,8 +123,8 @@ void Controller::UpdateSafe(){
 void Controller::GetNextController_Gain_Time_Index(double current_time){
     // Determine if a certain amount of time has passed, and if so, then increase the current_iteration_index and get the next K value
 
-    double switch_time = (controller_gain_times[current_iteration_index+1] - controller_gain_times[current_iteration_index]) / 2.0;
-    if(current_time - tvc_start_time > switch_time)
+    double switch_time = (controller_gain_times[current_iteration_index+1] + controller_gain_times[current_iteration_index]) / 2.0;
+    if(current_time - k_iteration_start_time > switch_time)
     {
         current_iteration_index++;
     }
@@ -173,11 +174,14 @@ void Controller::ImportControlParameters(std::string file_name){
             controller_gains(i, j) = stod(item);
         }
     }
-
 
     in.close();
 }
 
+void Controller::ResetKIteration(double current_time){
+    k_iteration_start_time = current_time;
+}
+
 Eigen::Matrix<double, 2, 8> Controller::GetCurrentKMatrix()
 {
     return controller_gains.block(current_iteration_index*2, 0, 2, 8);

diff --git a/src/Mode.cpp b/src/Mode.cpp
@@ -172,7 +172,7 @@ Mode::Phase Mode::UpdateLaunch(Navigation& navigation, Controller& controller, I
      }
 }
 
-Mode::Phase Mode::UpdateFreefall(Navigation& navigation, Igniter& igniter, double currentTime) {
+Mode::Phase Mode::UpdateFreefall(Navigation& navigation, Controller &controller, Igniter& igniter, double currentTime) {
 
     // Continue to update navigation
     navigation.UpdateNavigation();
@@ -198,6 +198,7 @@ Mode::Phase Mode::UpdateFreefall(Navigation& navigation, Igniter& igniter, doubl
     if (time_till_second_ignite < 0){
         igniter.Ignite(Igniter::IgnitionSpecifier::LAND);
         std::cout<< "Switching from Freefall to Land" << "\n";
+        controller.ResetKIteration(currentTime);
         return Mode::Land;
     }
 
@@ -272,7 +273,7 @@ bool Mode::Update(Navigation& navigation, Controller& controller, Igniter& ignit
             break;
         case Freefall:
             Telemetry::GetInstance().RunTelemetry(navigation, controller, 0.01, 0.08);
-            this->eCurrentMode = UpdateFreefall(navigation, igniter, currentTime);
+            this->eCurrentMode = UpdateFreefall(navigation, controller, igniter, currentTime);
             break;
         case Land:
             Telemetry::GetInstance().RunTelemetry(navigation, controller, 0.01, 0.08);