Stargazer 1.4 FSM update (#72)

mpkarpov-ui · SuragNuthulapaty · divijgarg · web-flow · commit 7757f0d14f82 · 2025-02-02T02:39:00.000-06:00
* initial safety changes * functionality * fig gpio bug * what is this buffer size bug * buzzer & LED checks * pyro reset & naming * reset fr this time * more change * transitions work * midas in flight control firing pyros * funny buzz * gitignore outputs * spreading factor and timeouts * manual cobine combiner code * trying to manually combine rx feather and this branch. DO NOT MERGE THIS CODE RIGHT NOW. IT DOES NOT WORK * new gss midas-commanding-flight changes * intermediate work * added potential orientation code, needs to be checked * Update Orientation.cpp * fixed syntax errors * Added Complimentary Filter to Orientation.cpp Co-authored-by: SZhOU-c <SZhOU-c@users.noreply.github.com> Co-authored-by: sh1shir <sh1shir@users.noreply.github.com> Co-authored-by: Rithvik Bhogavilli <rbhogavilli@gmail.com> * Fixed tilt, complementary filter not correct * landing buzzer (#71) * pls * added units --------- Co-authored-by: SuragNuthulapaty <77648585+SuragNuthulapaty@users.noreply.github.com> Co-authored-by: Divij <diviaops@gmail.com> Co-authored-by: keshavbalaji <keshavbalaji2397@gmail.com> Co-authored-by: SZhOU-c <SZhOU-c@users.noreply.github.com> Co-authored-by: sh1shir <sh1shir@users.noreply.github.com> Co-authored-by: Rithvik Bhogavilli <rbhogavilli@gmail.com>
diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp
@@ -130,7 +130,6 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
                 state = FSMState::STATE_SAFE;
             }
 
-
             break;
 
         case FSMState::STATE_IDLE:
@@ -181,6 +180,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
             break;
 
         case FSMState::STATE_SUSTAINER_IGNITION:
+            // This state probably does not need a pyro lockout, since we have a back-transition from STATE_SECOND_BOOST
             // another time transition into coast after a certain amount of time
             if ((current_time - sustainer_ignition_time) > sustainer_ignition_to_coast_timer_threshold) {
                 coast_time = current_time;
@@ -240,7 +240,12 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
 
         case FSMState::STATE_DROGUE_DEPLOY:
             // if detected a sharp change in jerk then go to next state
-            if (abs(state_estimate.jerk) < sustainer_drogue_jerk_threshold) {
+            // Drogue deploy should ALWAYS stay for at least some time, due to a lack of a back-transition from STATE_DROGUE
+            if((current_time - drogue_time) < sustainer_pyro_firing_time_minimum) {
+                break;
+            }
+
+            if (abs(state_estimate.jerk) > sustainer_drogue_jerk_threshold) {
                 state = FSMState::STATE_DROGUE;
                 break;
             }
@@ -261,9 +266,15 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
             break;
 
         case FSMState::STATE_MAIN_DEPLOY:
+            // Main deploy should ALWAYS stay for at least some time, due to a lack of a back-transition from STATE_DROGUE
+            if((current_time - drogue_time) < sustainer_pyro_firing_time_minimum) {
+                break;
+            }
+
             // if detected a sharp change in jerk then go to the next state
-            if (abs(state_estimate.jerk) < sustainer_main_jerk_threshold) {
+            if (abs(state_estimate.jerk) > sustainer_main_jerk_threshold) {
                 state = FSMState::STATE_MAIN;
+                main_deployed_time = current_time;
                 break;
             }
 
@@ -275,13 +286,18 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
 
         case FSMState::STATE_MAIN:
             // if slowed down enough then go on to the next state
-            if (abs(state_estimate.vertical_speed) <= sustainer_landed_vertical_speed_threshold) {
+            if ((abs(state_estimate.vertical_speed) <= sustainer_landed_vertical_speed_threshold) && (current_time - main_deployed_time) > sustainer_main_to_landed_lockout) {
                 landed_time = current_time;
                 state = FSMState::STATE_LANDED;
             }
             break;
 
         case FSMState::STATE_LANDED:
+
+            if((current_time - landed_time) > sustainer_landed_time_lockout) {
+                break;
+            }
+
             // if the slow speed was too brief then return to previous state
             if ((abs(state_estimate.vertical_speed) > sustainer_landed_vertical_speed_threshold) && ((current_time - landed_time) > sustainer_landed_timer_threshold)) {
                 state = FSMState::STATE_MAIN;
@@ -423,7 +439,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
             break;
 
         case FSMState::STATE_DROGUE_DEPLOY:
-            if (abs(state_estimate.jerk) < booster_drogue_jerk_threshold) {
+
+            // Drogue deploy should ALWAYS stay for at least some time, due to a lack of a back-transition from STATE_DROGUE
+            if((current_time - drogue_time) < booster_pyro_firing_time_minimum) {
+                break;
+            }
+
+            if (abs(state_estimate.jerk) > booster_drogue_jerk_threshold) {
                 state = FSMState::STATE_DROGUE;
                 break;
             }
@@ -441,8 +463,15 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
             break;
 
         case FSMState::STATE_MAIN_DEPLOY:
-            if (abs(state_estimate.jerk) < booster_main_jerk_threshold) {
+
+            // Main deploy should ALWAYS stay for at least some time, due to a lack of a back-transition from STATE_DROGUE
+            if((current_time - main_time) < booster_pyro_firing_time_minimum) {
+                break;
+            }
+
+            if (abs(state_estimate.jerk) > booster_main_jerk_threshold) {
                 state = FSMState::STATE_MAIN;
+                main_deployed_time = current_time;
                 break;
             }
 
@@ -452,13 +481,18 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
             break;
 
         case FSMState::STATE_MAIN:
-            if (abs(state_estimate.vertical_speed) <= booster_landed_vertical_speed_threshold) {
+            if (abs(state_estimate.vertical_speed) <= booster_landed_vertical_speed_threshold && (current_time - main_deployed_time) > booster_main_to_landed_lockout) {
                 landed_time = current_time;
                 state = FSMState::STATE_LANDED;
             }
             break;
 
         case FSMState::STATE_LANDED:
+
+            if((current_time - landed_time) > booster_landed_time_lockout) {
+                break;
+            }
+
             if ((abs(state_estimate.vertical_speed) > booster_landed_vertical_speed_threshold) && ((current_time - landed_time) > booster_landed_timer_threshold)) {
                 state = FSMState::STATE_MAIN;
             }
diff --git a/MIDAS/src/finite-state-machines/fsm.h b/MIDAS/src/finite-state-machines/fsm.h
@@ -43,6 +43,7 @@ class FSM {
     double drogue_time;
     double apogee_time;
     double main_time;
+    double main_deployed_time;
     double landed_time;
     double first_separation_time;
     double pyro_test_entry_time;
diff --git a/MIDAS/src/finite-state-machines/thresholds.h b/MIDAS/src/finite-state-machines/thresholds.h
@@ -11,69 +11,81 @@
 // SECOND STAGE THRESHOLDS
 // ----------------------------------
 
-// Transition to FIRST_BOOST if acceleration is greater than this
+// Regardless of sensor inputs, stay on pyro firing states for at LEAST this time. (ms)
+#define sustainer_pyro_firing_time_minimum 200
+
+// Transition to FIRST_BOOST if acceleration is greater than this (G)
 #define sustainer_idle_to_first_boost_acceleration_threshold 3
 
 // Return state to IDLE if not boosting for this amount of time (ms)
 #define sustainer_idle_to_first_boost_time_threshold 1000
 
-// Transition to SECOND_BOOST from SUSTAINER_IGNITION if acceleration greater than this
-#define sustainer_ignition_to_second_boost_acceleration_threshold 3
+// Transition to SECOND_BOOST from SUSTAINER_IGNITION if acceleration greater than this (G)
+#define sustainer_ignition_to_second_boost_acceleration_threshold 4
 
 // Return state to SECOND_BOOST if not boosting for this amount of time (ms)
 #define sustainer_second_boost_to_coast_time_threshold 1000
 
 // Transition to COAST if acceleration is less than this value (g)
 #define sustainer_coast_detection_acceleration_threshold 0.2
 
-// Reach apogee state when vertical speed is less than or equal to this value
+// Reach apogee state when vertical speed is less than or equal to this value (m/s)
 #define sustainer_coast_to_apogee_vertical_speed_threshold 15
 
-// Revert back to COAST if the vertical speed in apogee is too high (was 0 before which may have caused it keep jumping back to COAST)
+// Revert back to COAST if the vertical speed in apogee is too high (was 0 before which may have caused it keep jumping back to COAST) (m/s)
 #define sustainer_apogee_backto_coast_vertical_speed_threshold 10
 
-// Revert back to COAST if apogee was too brief
+// Revert back to COAST if apogee was too brief (ms)
 #define sustainer_apogee_check_threshold 1000
 
-// Move on to DROGUE_DEPLOT after being in apogee for this amount of time
+// Move on to DROGUE_DEPLOT after being in apogee for this amount of time (ms)
 #define sustainer_apogee_timer_threshold 1000
 
-// Move on to DROGUE after a second of reaching apogee
+// Move on to DROGUE after a second of reaching apogee (ms)
 #define sustainer_drogue_timer_threshold 3000
 
-// Move on to MAIN after passing this amount of time
+// Move on to MAIN after passing this amount of time (ms)
 #define sustainer_main_to_main_deploy_timer_threshold 3000
 
-// Height required to deploy the main parachutes
-#define sustainer_main_deploy_altitude_threshold 3000
+// Height required to deploy the main parachutes (m)
+#define sustainer_main_deploy_altitude_threshold 1006
 
 // Return to SUSTAINER_IGNITION if not in SECOND_BOOST for this amount of time (ms)
 #define sustainer_ignition_to_second_boost_time_threshold 1000
 
-// Transition straight to coast after a certain amount of time not detecting second stage boost
+// Transition straight to coast after a certain amount of time not detecting second stage boost (ms)
 #define sustainer_ignition_to_coast_timer_threshold 5000
 
-// Revert back to main if the landed was too short
+// Revert back to main if the landed was too short (ms)
 #define sustainer_landed_timer_threshold 5000
 
 // Return state to FIRST_BOOST if not in BURNOUT for this amount of time (ms)
 #define sustainer_first_boost_to_burnout_time_threshold 1000
 
-// Transition to LANDED from MAIN if vertical speed is less than this threshold
-#define sustainer_landed_vertical_speed_threshold 20
+// Transition to LANDED from MAIN if vertical speed is less than this threshold (m/s)
+#define sustainer_landed_vertical_speed_threshold 3
+
+// Lock out further transitions from LANDED after this much time passes in the LANDED state. (ms)
+#define sustainer_landed_time_lockout 60000
 
-// Stores a small jerk value
+// Prevent us from inadvertently entering the LANDED state when we're at a low velocity at main deploy. (ms)
+#define sustainer_main_to_landed_lockout 5000
+
+// Stores a small jerk value (m/s^3)
 #define sustainer_drogue_jerk_threshold 200
 
-// Stores a small jerk value
+// Stores a small jerk value (m/s^3)
 #define sustainer_main_jerk_threshold 300
 
 
 // ----------------------------------
 // FIRST STAGE THRESHOLDS
 // ----------------------------------
 
-// Transition to FIRST_BOOST if acceleration is greater than this
+// Regardless of sensor inputs, stay on pyro firing states for at LEAST this time. (ms)
+#define booster_pyro_firing_time_minimum 200
+
+// Transition to FIRST_BOOST if acceleration is greater than this (G)
 #define booster_idle_to_first_boost_acceleration_threshold 3
 
 // Return state to IDLE if not boosting for this amount of time (ms)
@@ -85,43 +97,51 @@
 // Transition to COAST if acceleration is less than this value (g)
 #define booster_coast_detection_acceleration_threshold 0.2
 
-// Reach apogee state when vertical speed is less than or equal to this value
+// Reach apogee state when vertical speed is less than or equal to this value (m/s)
 #define booster_coast_to_apogee_vertical_speed_threshold 20
 
-// Revert back to COAST if apogee was too brief
+// Revert back to COAST if apogee was too brief (ms)
 #define booster_apogee_check_threshold 1000
 
-// Move on to DROGUE_DEPLOT after being in apogee for this amount of time
+// Move on to DROGUE_DEPLOT after being in apogee for this amount of time (ms)
 #define booster_apogee_timer_threshold 1000
 
-// Move on to DROGUE after a second of reaching apogee
+// Move on to DROGUE after a second of reaching apogee (ms)
 #define booster_drogue_timer_threshold 3000
 
-// Move on to MAIN after passing this amount of time
+// Move on to MAIN after passing this amount of time (ms)
 #define booster_main_to_main_deploy_timer_threshold 3000
 
-// Height required to deploy the main parachutes
-#define booster_main_deploy_altitude_threshold 3000
+// Height required to deploy the main parachutes (m)
+// [STARGAZER 1.4] This is a "dontcare" value --> The booster does not have a drogue, we transition immediately to MAIN
+#define booster_main_deploy_altitude_threshold 999999
+
 // Return to SUSTAINER_IGNITION if not in SECOND_BOOST for this amount of time (ms)
 #define booster_ignition_to_second_boost_time_threshold 1000
 
-// Transition straight to coast after a certain amount of time not detecting second stage boost
+// Transition straight to coast after a certain amount of time not detecting second stage boost (ms)
 #define booster_ignition_to_coast_timer_threshold 5000
 
-// Revert back to main if the landed was too short
+// Revert back to main if the landed was too short (ms)
 #define booster_landed_timer_threshold 5000
 
 // Return state to FIRST_BOOST if not in BURNOUT for this amount of time (ms)
 #define booster_first_boost_to_burnout_time_threshold 1000
 
-// Transition to LANDED from MAIN if vertical speed is less than this threshold
-#define booster_landed_vertical_speed_threshold 20
+// Transition to LANDED from MAIN if vertical speed is less than this threshold (G)
+#define booster_landed_vertical_speed_threshold 4
+
+// Lock out further transitions from LANDED after this much time passes in the LANDED state. (ms)
+#define booster_landed_time_lockout 60000
+
+// Prevent us from inadvertently entering the LANDED state when we're at a low velocity at main deploy. (ms)
+#define booster_main_to_landed_lockout 5000
 
-// Stores a small jerk value
+// Stores a small jerk value (m/s^3)
 #define booster_first_separation_jerk_threshold 300
 
-// Stores a small jerk value
+// Stores a small jerk value (m/s^3)
 #define booster_drogue_jerk_threshold 200
 
-// Stores a small jerk value
+// Stores a small jerk value (m/s^3)
 #define booster_main_jerk_threshold 300
diff --git a/MIDAS/src/hardware/Orientation.cpp b/MIDAS/src/hardware/Orientation.cpp
@@ -4,7 +4,6 @@
 #include "sensor_data.h"
 #include <cmath>
 
-
 // global static instance of the sensor
 Adafruit_BNO08x imu(BNO086_RESET);
 #define REPORT_INTERVAL_US 5000
@@ -34,6 +33,12 @@ ErrorCode OrientationSensor::init()
     return ErrorCode::NoError;
 }
 
+float angle_between_quaternions(const Quaternion &q1, const Quaternion &q2)
+{
+    float dot_product = Quaternion::dot(q1, q2);
+    return 2.0f * std::acos(std::fabs(dot_product));
+}
+
 /**
  * @brief Turns a quaternion into its corresponding Euler 3D vector representation
  *
@@ -87,6 +92,27 @@ Vec3 quaternionToEulerGI(sh2_GyroIntegratedRV_t *rotational_vector, bool degrees
                              degrees);
 }
 
+std::tuple<float, float, float> euler_to_vector(float pitch, float yaw)
+{
+    float x = cos(pitch) * cos(yaw);
+    float y = cos(pitch) * sin(yaw);
+    float z = sin(pitch);
+    return {x, y, z};
+}
+
+float angular_difference(float pitch1, float yaw1, float pitch2, float yaw2)
+{
+    auto [x1, y1, z1] = euler_to_vector(pitch1, yaw1);
+    auto [x2, y2, z2] = euler_to_vector(pitch2, yaw2);
+
+    float mag1 = sqrt(x1 * x1 + y1 * y1 + z1 * z1);
+    float mag2 = sqrt(x2 * x2 + y2 * y2 + z2 * z2);
+
+    float dot_product = x1 * x2 + y1 * y2 + z1 * z2;
+
+    return std::acos(dot_product / (mag1 * mag2));
+}
+
 /**
  * @brief Generates a rotation matrix that transforms a vector by the rotations described in rpy_vec {roll, pitch, yaw} (in that order!)
  */
@@ -156,6 +182,11 @@ Orientation OrientationSensor::read()
         
         Orientation sensor_reading;
         sensor_reading.has_data = true;
+        /*
+        sensor_reading.yaw = -filtered_euler.y;
+        sensor_reading.pitch = filtered_euler.x;
+        sensor_reading.roll = filtered_euler.z;
+        */
 
         sensor_reading.yaw = -euler.y;
         sensor_reading.pitch = euler.x;
@@ -179,6 +210,7 @@ Orientation OrientationSensor::read()
         if (initial_flag == 0)
         {
             initial_orientation = sensor_reading;
+            initial_quaternion = quat;
             initial_flag = 1;
         }
 
diff --git a/MIDAS/src/hardware/sensors.h b/MIDAS/src/hardware/sensors.h
@@ -67,6 +67,7 @@ struct VoltageSensor {
  */
 struct OrientationSensor {
     Orientation initial_orientation;
+    Quaternion initial_quaternion;
     uint8_t initial_flag;
 
     float prev_x = 0;
diff --git a/MIDAS/src/sensor_data.h b/MIDAS/src/sensor_data.h
@@ -170,6 +170,17 @@ struct Magnetometer {
     float mz;
 };
 
+struct Quaternion {
+    float w, x, y, z;
+
+    static float dot(const Quaternion& q1, const Quaternion& q2) {
+        return q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
+    }
+
+
+
+};
+
 /**
  * @struct Orientation
  * 
diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp
diff --git a/ground/src/feather/main.cpp b/ground/src/feather/main.cpp
