created control template variables and created a block that updates t…

src/main.ino

@@ -196,7 +196,6 @@ void setup()
     // set up controller
     // TODO : move stuff below
     ws.x << 0, 0, 0, 0, 0, 0; //state vector
-    ws.Ka << 0.34641, 1.72254, 0.32694, 0, 0, 0, 0, 0, 0, 0.34641, -1.88376, -0.3991; //LQR gains
     ws.uLast[0] = 0;  //last commanded tvc x input
     ws.uLast[1] = 0; //last commanded tvc y input
 
@@ -241,7 +240,9 @@ void loop()
             }
             else
             {
-                // TODO: implement this block
+                //construct x
+                //construct y
+                ws.x+=ws.dt*(ws.A*ws.x+ws.B*ws.u+ws.L*(ws.y-ws.C*ws.x)) //calculate next state
             }
             break;
         }
@@ -254,7 +255,22 @@ void loop()
             }
             else
             {
-                // TODO: implement this block
+                //checks whether the burn time has surpassed the pre-defined burn interval
+                //if so, then updates the control constant by multiplying by the current thrust
+                if (ws.thrust_curve_count+1<T_intervals && ws.t_prev_cycle>(ws.fire_time+T_time[ws.thrust_curve_count+1])*MEGA)
+                {
+                    ws.thrust_curve_count++;
+                    ws.K=K_template*T[ws.thrust_curve_count];
+                    ws.A=A_template*T[ws.thrust_curve_count];
+                    ws.B=B_template*T[ws.thrust_curve_count];
+                    ws.L=L_template*T[ws.thrust_curve_count];
+                }
+                //construct x
+                //construct y
+                ws.u=-ws.K*ws.x; //calculate input
+                ws.x+=ws.dt*(ws.A*ws.x+ws.B*ws.u+ws.L*(ws.y-ws.C*ws.x)); //calculate next state
+                //process u
+                //send u to tvc
             }
             break;
         }
@@ -264,14 +280,6 @@ void loop()
             break;
         }
     }
-    // TODO: formalize control loop
-    //construct x
-    //construct y
-    ws.u=-ws.K*ws.x;  //calculate tvc input
-    ws.x+=ws.dt*(ws.A*ws.x+ws.B*ws.u+ws.L*(ws.y-ws.C*ws.x)) //calculate next state estimate
-    //send u to tvc
-
-
     // TODO: add blink
     // TODO: add data record
     // TODO: add (somewhere else) data struct

src/mission_constants.hh

@@ -2,6 +2,8 @@
 #define __MISSION_CONSTANTS_HH__
 
 #include <cstdint>
+#include <Eigen313.h>  // linear algebra library
+#include <LU>  // needed for Eigen313.h to work
 
 
 //*****************************************************************************
@@ -107,4 +109,19 @@ const float QR_IMU_TO_BODY[4] = {1.0, 0.0, 0.0, 0.0};  // (--) right, scalar-fir
 const float QR_BODY_TO_IMU[4] = {1.0, 0.0, 0.0, 0.0};  // (--) right, scalar-first, Hamiltonian quaternion transforming body frame to IMU frame
 
 
+//control constants
+MatrixXf A_template(36); //dynamics matrix
+MatrixXf B_template(12); //input matrix
+MatrixXf L_template(36); //Kalman gain
+MatrixXf K_template(12); //LQR -> updates mid flight
+MatrixXf C(36); //Sensor matrix
+C=MatrixXd::Identity(6, 6);
+
+//thrust curve
+const float T={30, 9}; //Thrust values (in Newtons)
+const float T_time={0, .33}; //time in the burn when we start to use the corresponding thrust value
+const int T_intervals=2; //how many thrust data points do we have
+
+
+
 #endif  // __MISSION_CONSTANTS_HH__

src/workspace.hh

@@ -40,6 +40,10 @@ class Workspace
         unsigned long t_prev_cycle = 0;  // (us) contains the time of the previous cycle at the start of each loop
         unsigned long dt = 0;  // (us) used to store time difference between t_prev_cycle and return of micros() at the start of each loop
 
+        //event times
+        unsigned long drop_time=0;
+        unsigned long fire_time=0;
+
         // sensor measurements
         float a_0[3] = {0.0, 0.0, 0.0};  // (m/s^2) linear acceleration, used for storing sensor measurements
         float a_1[3] = {0.0, 0.0, 0.0};  // (m/s^2) linear acceleration, used for storing sensor measurements
@@ -54,17 +58,18 @@ class Workspace
         Servo tvc_x;  // servo that actuates TVC around x body axis
         Servo tvc_y;  // servo that actuates TVC around x body axis
 
-        // controller parameters
-        // TODO: define parameters, move to mission constants
-        MatrixXf K(2,6); //LQR -> updates mid flight
+        //control vectors
         VectorXi x(6); //state vector = {vx, theta_y, d_theta_y_dt, vy, theta_x, d_theta_x_dt} global frame and euler angles
         VectorXf u(2); //input vector
+        float maxU = 5*DEG_2_RAD;  //maximum gimbal angle
+        int uLast[2] = {0, 0};  //commanded servo angle
+
+        //control constants -> these are variable throughout the flight -> multiples of the templates in mission constants
         MatrixXf A(36); //dynamics matrix
         MatrixXf B(12); //input matrix
-        MatrixXf C(24); //Sensor matrix
-        MatrixXf L(24); //Kalman gain
-        float maxU = 5*3.14159/180;  //maximum gimbal angle
-        long uLast[2] = {0, 0};  //commanded servo angle
+        MatrixXf L(36); //Kalman gain
+        MatrixXf K(12); //LQR gain
+        int thrust_curve_count=0; //current index of the thrust curve array
 
         // state
         float r_body[3] = {0.0, 0.0, 0.0};  // (m) position of the body frame origin TODO: define inertial frame