prep data implementation

Propulsive-Landing · Mar 27, 2021 · fb3421a · fb3421a
1 parent bed5067
commit fb3421a
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 2 deletions.
diff --git a/main/main.ino b/main/main.ino
@@ -314,6 +314,9 @@ void loop()
     main_display_matrix(ws.x, 1);
     //Serial.print(millis());
     //main_display_matrix(ws.last_u, RAD_2_DEG);
+
+    ws.construct_data();
+
     // TODO: add data record
     // TODO: add (somewhere else) data struct
 }
diff --git a/main/mission_constants.hh b/main/mission_constants.hh
@@ -117,8 +117,8 @@ const vector<float> B_VALUES {THRUST/MASS, 0,
                         0, -THRUST*MOMENT_ARM/MOMENT_INERTIA_XX}; //input matrix
 const Matrix B=Matrix(6, 2, B_VALUES);
 
-const vector<float> K_VALUES {-.6325,-5.5608,-1.4869,0,0,0,
-                        0,0,0,.6325,-5.5608,-1.4869}; //controller gain
+const vector<float> K_VALUES {-.6325,-5.2871,-1.3423,0,0,0,
+                        0,0,0,.6325,-5.2871,-1.3423}; //controller gain
 const Matrix KC=Matrix(2, 6, K_VALUES);
 
 const vector<float> C_VALUES {1,0,0,0,0,0,

diff --git a/main/workspace.hh b/main/workspace.hh
@@ -10,6 +10,18 @@
 #include <vector>
 using namespace std;
 
+struct loop_data
+{
+    int mode;
+    long int time;
+    float dt;
+    vector<float> x(6, 0);
+    vector<float> y(4, 0);
+    vector<float> u(2, 0);
+    float yaw;
+    float theta_raw_0 [3];
+    float theta_raw_1 [3];
+}
 
 /* Workspace
  *
@@ -39,6 +51,7 @@ class Workspace
         unsigned long calibrate_time = 0;  //momment main loop starts
         unsigned long t_prev_cycle = 0;  // (us) contains the time of the previous cycle at the start of each loop
         float dt = 0;  // (us) used to store time difference between t_prev_cycle and return of micros() at the start of each loop
+        struct loop_data current_data;
 
         // sensor measurements
         float a_0[3] = {0.0, 0.0, 0.0};  // (m/s^2) linear acceleration, used for storing sensor measurements
@@ -53,6 +66,7 @@ class Workspace
         // Servos
         Servo tvc_x;  // servo that actuates TVC around x body axis
         Servo tvc_y;  // servo that actuates TVC around x body axis
+        bool tvc_alternate=false; //
 
         //control vectors
         // set up controller
@@ -111,6 +125,19 @@ class Workspace
                 x.values=x_raw.values;
             }
         }
+
+        void construct_data()
+        {
+            current_data.dt=dt;
+            current_data.mode=mode;
+            current_data.theta_raw_0=theta_0;
+            current_data.theta_raw_1=theta_1;
+            current_data.x=x;
+            current_data.y=y;
+            current_data.u=last_u;
+            current_data.yaw=yaw;
+            current_data.time=t_prev_cycle;
+        }
 };
 
 #endif  // __WORKSPACE_HH__