Sb/ekf cleanup

MIDAS/src/gnc/aero_coeff.h

@@ -0,0 +1,125 @@
+/** 
+ * This file contains all the aerodynamic constants
+ * aero_data - the aerodynamic coefficients for drag calculations
+ * thurst curves for both motors
+ * 
+ */
+typedef struct
+{
+    float mach;
+    float alpha;
+    float CA_power_on;
+    float CN;
+    float CP;
+} aero_coeff_t;
+
+// stores the aerodynamic coefficients for the corresponding Mach number
+const aero_coeff_t aero_data[] = {
+    {0.04, 0, 1.000001789, 25.80486518, 123.856999},
+    {0.08, 0, 0.899149955, 25.80486518, 123.856999},
+    {0.12, 0, 0.848262793, 25.80486518, 123.856999},
+    {0.16, 0, 0.815490497, 25.80486518, 123.856999},
+    {0.2, 0, 0.791977907, 25.80486518, 123.856999},
+    {0.24, 0, 0.77407759, 25.80486518, 123.856999},
+    {0.28, 0, 0.763046286, 25.80486518, 123.856999},
+    {0.32, 0, 0.758319846, 25.80486518, 123.856999},
+    {0.36, 0, 0.760511343, 25.80486518, 123.856999},
+    {0.4, 0, 0.763737136, 25.80486518, 123.856999},
+    {0.44, 0, 0.767325178, 25.80486518, 123.856999},
+    {0.48, 0, 0.771334851, 25.80486518, 123.856999},
+    {0.52, 0, 0.775843406, 25.80486518, 123.856999},
+    {0.56, 0, 0.780953377, 25.80486518, 123.856999},
+    {0.6, 0, 0.785771581, 25.80486518, 123.856999},
+    {0.64, 0, 0.793730593, 25.80486518, 123.856999},
+    {0.68, 0, 0.80285965, 25.80486518, 123.856999},
+    {0.72, 0, 0.807910063, 25.80486518, 123.856999},
+    {0.76, 0, 0.807403195, 25.80486518, 123.856999},
+    {0.8, 0, 0.806889479, 25.80486518, 123.856999},
+    {0.84, 0, 0.832707826, 25.80486518, 123.856999},
+    {0.88, 0, 0.858519521, 25.80486518, 123.856999},
+    {0.92, 0, 0.895125486, 25.492166, 124.3408619},
+    {0.96, 0, 0.923744595, 24.86676763, 125.3085876},
+    {1, 0, 0.941214699, 24.24136926, 126.2763132},
+};
+
+// Moonburner motor thrust curve (Sustainer)
+const std::map<float, float> M685W_data = {
+    {0.13, 1368.376},
+    {0.249, 1361.395},
+    {0.308, 1380.012},
+    {0.403, 1359.068},
+    {0.675, 1184.53},
+    {1.018, 1072.826},
+    {1.456, 996.029},
+    {1.977, 958.794},
+    {2.995, 914.578},
+    {3.99, 856.399},
+    {4.985, 781.929},
+    {5.494, 730.732},
+    {5.991, 679.534},
+    {7.258, 542.231},
+    {7.862, 463.107},
+    {8.015, 456.125},
+    {8.998, 330.458},
+    {9.993, 207.118},
+    {10.514, 137.303},
+    {11.496, 34.908},
+    {11.994, 0.0}
+};
+
+// O5500X motor thrust curve (Booster)
+const std::map<float, float> O5500X_data = {
+    {0.044, 7112.245},
+    {0.063, 6734.694},
+    {0.078, 6897.959},
+    {0.094, 6612.245},
+    {0.109, 6765.306},
+    {0.125, 6540.816},
+    {0.147, 6581.633},
+    {0.194, 6520.408},
+    {0.35, 6795.918},
+    {0.428, 7091.837},
+    {0.563, 7285.714},
+    {0.694, 7408.163},
+    {0.988, 7581.633},
+    {1.266, 7622.449},
+    {1.491, 7724.49},
+    {1.581, 7653.061},
+    {1.641, 7540.816},
+    {1.684, 7500.0},
+    {1.716, 7336.735},
+    {1.784, 7224.49},
+    {1.938, 6785.714},
+    {2.138, 6326.531},
+    {2.491, 5897.959},
+    {2.6, 5704.082},
+    {2.919, 3540.816},
+    {3.022, 3408.163},
+    {3.138, 2887.755},
+    {3.3, 2234.694},
+    {3.388, 1673.469},
+    {3.441, 1489.796},
+    {3.544, 1418.367},
+    {3.609, 1295.918},
+    {3.688, 816.327},
+    {3.778, 653.061},
+    {3.819, 581.633},
+    {3.853, 489.796},
+    {3.897, 285.714},
+    {3.981, 20.408},
+    {3.997, 0.0}
+};
+
+// constant variable that contains the booster and sustainer motors
+const std::map<std::string, std::map<float, float>> motor_data = {
+    {"Booster", O5500X_data},
+    {"Sustainer", M685W_data}
+};
+
+/**
+ * @brief linearly interpolates the a value based on the lower and upper bound, similar to lerp_() in PySim
+ */
+inline float linearInterpolation(float x0, float y0, float x1, float y1, float x)
+{
+    return y0 + ((x - x0) * (y1 - y0) / (x1 - x0));
+}

MIDAS/src/gnc/constants.h

@@ -0,0 +1,10 @@
+// constants
+const float pi = 3.14159268;
+const float a = 343.0;                // (m/s) speed of sound
+const float rho = 1.225;              // average air density
+const float r = 0.03935;              // (m)
+const float height_full = 4.457;      // (m) height of rocket Full Stage
+const float height_sustainer = 2.029; // (m) height of rocket Sustainer
+const float mass_full = 33.84;        // (kg) Sustainer + Booster
+const float mass_sustainer = 10.93;   // (kg) Sustainer
+const float gravity_ms2 = 9.81;           // (m/s^2) accel due to gravity