new plantsim

Alpha-CubeSat · Oct 30, 2023 · e46f608 · e46f608
1 parent 7b9087a
commit e46f608
Show file tree

Hide file tree

Showing 6 changed files with 21 additions and 26 deletions.
diff --git a/lib/Plant_ert_rtw/Plant.cpp b/lib/Plant_ert_rtw/Plant.cpp
@@ -69,6 +69,8 @@ real_T wz =1;                       // Variable: wz
                                           //  Referenced by: '<S1>/Discrete-Time Integrator2'
                                           //  init angular vel -z
 
+double f_step_size=0.001;
+
 extern real_T rt_atan2d_snf(real_T u0, real_T u1);
 extern void rt_invd3x3_snf(const real_T u[9], real_T y[9]);
 
@@ -582,19 +584,19 @@ void Plant::step()
   rtDW.DiscreteTimeIntegrator1_DSTAT_m[0] = ((rtb_Product[3] *
     rtDW.DiscreteTimeIntegrator_DSTATE - rtb_Product[2] *
     rtDW.DiscreteTimeIntegrator1_DSTATE) + rtb_Product[1] *
-    rtDW.DiscreteTimeIntegrator2_DSTATE) / 2.0 * 0.001 + rtb_Product_n;
+    rtDW.DiscreteTimeIntegrator2_DSTATE) / 2.0 * f_step_size + rtb_Product_n;
   rtDW.DiscreteTimeIntegrator1_DSTAT_m[1] = ((rtb_Product[2] *
     rtDW.DiscreteTimeIntegrator_DSTATE + rtb_Product[3] *
     rtDW.DiscreteTimeIntegrator1_DSTATE) - rtb_Product[0] *
-    rtDW.DiscreteTimeIntegrator2_DSTATE) / 2.0 * 0.001 + rtb_qd2;
+    rtDW.DiscreteTimeIntegrator2_DSTATE) / 2.0 * f_step_size + rtb_qd2;
   rtDW.DiscreteTimeIntegrator1_DSTAT_m[2] = ((-rtb_Product[1] *
     rtDW.DiscreteTimeIntegrator_DSTATE + rtb_Product[0] *
     rtDW.DiscreteTimeIntegrator1_DSTATE) + rtb_Product[3] *
-    rtDW.DiscreteTimeIntegrator2_DSTATE) / 2.0 * 0.001 + rtb_qd3;
+    rtDW.DiscreteTimeIntegrator2_DSTATE) / 2.0 * f_step_size + rtb_qd3;
   rtDW.DiscreteTimeIntegrator1_DSTAT_m[3] = ((-rtb_Product[0] *
     rtDW.DiscreteTimeIntegrator_DSTATE - rtb_Product[1] *
     rtDW.DiscreteTimeIntegrator1_DSTATE) - rtb_Product[2] *
-    rtDW.DiscreteTimeIntegrator2_DSTATE) / 2.0 * 0.001 + acc;
+    rtDW.DiscreteTimeIntegrator2_DSTATE) / 2.0 * f_step_size + acc;
 
   // End of Outputs for SubSystem: '<S1>/Quaternion Integration'
 
@@ -652,32 +654,32 @@ void Plant::step()
   //   Product: '<S7>/Divide1'
 
   rtDW.DiscreteTimeIntegrator1_DSTAT_l += 1.0 / rtb_qd4 *
-    rtDW.DiscreteTimeIntegrator5_DSTATE * acc * 0.001;
+    rtDW.DiscreteTimeIntegrator5_DSTATE * acc * f_step_size;
 
   // Update for DiscreteIntegrator: '<S7>/Discrete-Time Integrator2' incorporates:
   //   DiscreteIntegrator: '<S7>/Discrete-Time Integrator4'
   //   DotProduct: '<S7>/Dot Product5'
   //   Product: '<S7>/Divide1'
 
   rtDW.DiscreteTimeIntegrator2_DSTAT_e += 1.0 / rtb_qd4 *
-    rtDW.DiscreteTimeIntegrator4_DSTATE * acc * 0.001;
+    rtDW.DiscreteTimeIntegrator4_DSTATE * acc * f_step_size;
 
   // Update for DiscreteIntegrator: '<S7>/Discrete-Time Integrator3' incorporates:
   //   DiscreteIntegrator: '<S7>/Discrete-Time Integrator6'
   //   DotProduct: '<S7>/Dot Product3'
   //   Product: '<S7>/Divide1'
 
   rtDW.DiscreteTimeIntegrator3_DSTATE += 1.0 / rtb_qd4 *
-    rtDW.DiscreteTimeIntegrator6_DSTATE * acc * 0.001;
+    rtDW.DiscreteTimeIntegrator6_DSTATE * acc * f_step_size;
 
   // Update for DiscreteIntegrator: '<S7>/Discrete-Time Integrator4'
-  rtDW.DiscreteTimeIntegrator4_DSTATE += 0.001 * rtb_DiscreteTimeIntegrator2;
+  rtDW.DiscreteTimeIntegrator4_DSTATE += f_step_size * rtb_DiscreteTimeIntegrator2;
 
   // Update for DiscreteIntegrator: '<S7>/Discrete-Time Integrator5'
-  rtDW.DiscreteTimeIntegrator5_DSTATE += 0.001 * rtb_DiscreteTimeIntegrator1;
+  rtDW.DiscreteTimeIntegrator5_DSTATE += f_step_size * rtb_DiscreteTimeIntegrator1;
 
   // Update for DiscreteIntegrator: '<S7>/Discrete-Time Integrator6'
-  rtDW.DiscreteTimeIntegrator6_DSTATE += 0.001 * rtb_DiscreteTimeIntegrator3;
+  rtDW.DiscreteTimeIntegrator6_DSTATE += f_step_size * rtb_DiscreteTimeIntegrator3;
 
   // End of Outputs for SubSystem: '<S1>/Tranlational Dynamics'
 
@@ -879,13 +881,13 @@ void Plant::step()
   rtb_Normalization[2] *= acc;
 
   // Update for DiscreteIntegrator: '<S1>/Discrete-Time Integrator'
-  rtDW.DiscreteTimeIntegrator_DSTATE += 0.001 * rtb_Product_j[0];
+  rtDW.DiscreteTimeIntegrator_DSTATE += f_step_size * rtb_Product_j[0];
 
   // Update for DiscreteIntegrator: '<S1>/Discrete-Time Integrator1'
-  rtDW.DiscreteTimeIntegrator1_DSTATE += 0.001 * rtb_Product_j[1];
+  rtDW.DiscreteTimeIntegrator1_DSTATE += f_step_size * rtb_Product_j[1];
 
   // Update for DiscreteIntegrator: '<S1>/Discrete-Time Integrator2'
-  rtDW.DiscreteTimeIntegrator2_DSTATE += 0.001 * rtb_Product_j[2];
+  rtDW.DiscreteTimeIntegrator2_DSTATE += f_step_size * rtb_Product_j[2];
 
   // End of Outputs for SubSystem: '<Root>/Plant'
 
@@ -977,7 +979,7 @@ void Plant::initialize()
   }
 }
 // Model initialize function
-void Plant::initialize(double Altitude_input, double I_input[9], double inclination_input, double m_input, double q0_input[4], double wx_input, double wy_input, double wz_input)
+void Plant::initialize(double f_step_size_input, double Altitude_input, double I_input[9], double inclination_input, double m_input, double q0_input[4], double wx_input, double wy_input, double wz_input)
 {
   // Registration code
 
@@ -1007,6 +1009,8 @@ void Plant::initialize(double Altitude_input, double I_input[9], double inclinat
     wy = wy_input;
     wz = wz_input;
 
+    f_step_size=f_step_size_input;
+
     // SystemInitialize for Atomic SubSystem: '<Root>/Plant'
     // InitializeConditions for DiscreteIntegrator: '<S1>/Discrete-Time Integrator'
     rtDW.DiscreteTimeIntegrator_DSTATE = wx_input;

diff --git a/lib/Plant_ert_rtw/Plant.h b/lib/Plant_ert_rtw/Plant.h
@@ -206,7 +206,7 @@ class Plant
 
   // model initialize function
   void initialize();
-  void initialize(double Altitude_input, double I_input[9], double inclination_input, double m_input, double q0_input[4], double wx_input, double wy_input, double wz_input);
+  void initialize(double f_step_size_input, double Altitude_input, double I_input[9], double inclination_input, double m_input, double q0_input[4], double wx_input, double wy_input, double wz_input);
   // model step function
   void step();
 

diff --git a/lib/Plant_ert_rtw/Plant.o b/lib/Plant_ert_rtw/Plant.o
diff --git a/lib/Plant_ert_rtw/Plant_data.o b/lib/Plant_ert_rtw/Plant_data.o
diff --git a/src/Control Tasks/ACSControlTask.cpp b/src/Control Tasks/ACSControlTask.cpp
@@ -9,7 +9,7 @@ void ACSControlTask::execute()
 
 #ifdef ACS_SIM
     if (first) {
-        plantObj.initialize(altitude_input, I_input, inclination_input, m_input, q0_input, wx_input, wy_input, wz_input);
+        plantObj.initialize(0.001, altitude_input, I_input, inclination_input, m_input, q0_input, wx_input, wy_input, wz_input);
     }
 #endif
 
@@ -51,15 +51,7 @@ void ACSControlTask::execute()
     Serial.println(gyro_z);
 #endif
 
-    if (last_time == 0) {
-        plantObj.step();
-    } else {
-        num_steps = millis() - last_time;
-        for (int i = 0; i < num_steps; i++) {
-            plantObj.step();
-        }
-    }
-    last_time = millis();
+    plantObj.step();
 #endif
 
     if (!sfr::acs::off) {

diff --git a/src/Control Tasks/ACSControlTask.hpp b/src/Control Tasks/ACSControlTask.hpp
@@ -54,7 +54,6 @@ class ACSControlTask
     double wx_input = 0.0;
     double wy_input = 0.0;
     double wz_input = 1.0;
-    float last_time = 0;
 #endif
 };