Functionality for dual deployment

flight-computer.ino

@@ -8,7 +8,8 @@
 LSM9DS1_API imu_sensor = LSM9DS1_API::getInstance();
 BMP3XX_API altimeter = BMP3XX_API::getInstance();
 Telemetry telemetry = Telemetry::getInstance();
-MosfetIgniter igniter = MosfetIgniter::getInstance();
+MosfetIgniter drogueIgniter(1);
+MosfetIgniter mainIgniter(2);
 Adafruit_GPS_API gps = Adafruit_GPS_API::getInstance();
 FSM *fsm;
 // TODO: improve buzzer code
@@ -26,7 +27,7 @@ void setup() {
   tone(buzzer, 500);
 #endif
 
-  fsm = new FSM(&telemetry, &imu_sensor, &altimeter, &gps, &igniter, &loop_frequency);
+  fsm = new FSM(&telemetry, &imu_sensor, &altimeter, &gps, &drogueIgniter, &mainIgniter, &loop_frequency);
 
 #if BUZZER
   noTone(buzzer);

libraries/definitions.h

@@ -1,5 +1,4 @@
 #define SERIAL_DEBUG false
 #define SD_LOGS true
 #define IGN_DEBUG true
-#define IGNITER 1 // 1 or 2
 #define BUZZER true

libraries/fsm.cpp

@@ -12,15 +12,25 @@
 #define GRAVITY 372 // cm/s^2 -- Mars
 #define GRAVITY 162 // cm/s^2 -- Moon
 */
+#define MAIN_CHUTE_ALTITUDE 600 // meters
+#define MAIN_CHUTE_TIMEOUT 50 // seconds
 #define VBATPIN A7
-#define EJECTION_TIMEOUT 4000 // ms
-
-FSM::FSM(Telemetry* telemetry, IMU* imu_sensor, Altimeter* altimeter, GPSReceiver* gps, Igniter* igniter, uint8_t* loop_frequency)
+#define PYRO_TIMEOUT 4000 // ms
+
+FSM::FSM(
+  Telemetry* telemetry,
+  IMU* imu_sensor,
+  Altimeter* altimeter,
+  GPSReceiver* gps,
+  Igniter* drogueIgniter,
+  Igniter* mainIgniter,
+  uint8_t* loop_frequency)
   : telemetry(telemetry)
   , imu_sensor(imu_sensor)
   , altimeter(altimeter)
   , gps(gps)
-  , igniter(igniter)
+  , drogueIgniter(drogueIgniter)
+  , mainIgniter(mainIgniter)
   , loop_frequency(loop_frequency)
 {
   Transition flight_state_transitions[] = {
@@ -35,22 +45,25 @@ FSM::FSM(Telemetry* telemetry, IMU* imu_sensor, Altimeter* altimeter, GPSReceive
 
     // Ejection Test
     Transition(STATE::EJECTION_TEST_READY, EVENT::TRIGGER_FTS, STATE::EJECTION_TEST_EJECT),
-    Transition(STATE::EJECTION_TEST_EJECT, EVENT::CHUTE_EJECTED, STATE::EJECTION_TEST_COMPLETE),
+    Transition(STATE::EJECTION_TEST_EJECT, EVENT::DROGUE_DEPLOYED, STATE::EJECTION_TEST_COMPLETE),
 
     // Flying
     Transition(STATE::ASCENDING, EVENT::APOGEE_TIMER_TIMEOUT, STATE::APOGEE_TIMEOUT),
     Transition(STATE::ASCENDING, EVENT::INIT_CALIBRATION, STATE::CALIBRATION),
-    Transition(STATE::ASCENDING, EVENT::APOGEE_DETECTED, STATE::DEPLOYING_CHUTE),
-    Transition(STATE::APOGEE_TIMEOUT, EVENT::APOGEE_DETECTED, STATE::DEPLOYING_CHUTE),
-    Transition(STATE::DEPLOYING_CHUTE, EVENT::CHUTE_EJECTED, STATE::RECOVERING),
+    Transition(STATE::ASCENDING, EVENT::APOGEE_DETECTED, STATE::DEPLOYING_DROGUE),
+    Transition(STATE::APOGEE_TIMEOUT, EVENT::APOGEE_DETECTED, STATE::DEPLOYING_DROGUE),
+    Transition(STATE::DEPLOYING_DROGUE, EVENT::DROGUE_DEPLOYED, STATE::WAITING_FOR_MAIN),
+    Transition(STATE::WAITING_FOR_MAIN, EVENT::MAIN_CHUTE_TIMER_TIMEOUT, STATE::DEPLOYING_MAIN),
+    Transition(STATE::WAITING_FOR_MAIN, EVENT::REACHED_MAIN_CHUTE_ALTITUDE, STATE::DEPLOYING_MAIN),
+    Transition(STATE::DEPLOYING_MAIN, EVENT::MAIN_DEPLOYED, STATE::RECOVERING),
 
     // FTS from all states except IDLE
-    Transition(STATE::SETUP, EVENT::TRIGGER_FTS, STATE::DEPLOYING_CHUTE),
-    Transition(STATE::CALIBRATION, EVENT::TRIGGER_FTS, STATE::DEPLOYING_CHUTE),
-    Transition(STATE::READY, EVENT::TRIGGER_FTS, STATE::DEPLOYING_CHUTE),
-    Transition(STATE::ASCENDING, EVENT::TRIGGER_FTS, STATE::DEPLOYING_CHUTE),
-    Transition(STATE::APOGEE_TIMEOUT, EVENT::TRIGGER_FTS, STATE::DEPLOYING_CHUTE),
-    Transition(STATE::RECOVERING, EVENT::TRIGGER_FTS, STATE::DEPLOYING_CHUTE)
+    Transition(STATE::SETUP, EVENT::TRIGGER_FTS, STATE::DEPLOYING_DROGUE),
+    Transition(STATE::CALIBRATION, EVENT::TRIGGER_FTS, STATE::DEPLOYING_DROGUE),
+    Transition(STATE::READY, EVENT::TRIGGER_FTS, STATE::DEPLOYING_DROGUE),
+    Transition(STATE::ASCENDING, EVENT::TRIGGER_FTS, STATE::DEPLOYING_DROGUE),
+    Transition(STATE::APOGEE_TIMEOUT, EVENT::TRIGGER_FTS, STATE::DEPLOYING_DROGUE),
+    Transition(STATE::RECOVERING, EVENT::TRIGGER_FTS, STATE::DEPLOYING_DROGUE)
   };
 
   register_state_transitions(flight_state_transitions);
@@ -75,10 +88,14 @@ void FSM::process_event(EVENT event) {
   if (state_transitions[(int)state][(int)event] != STATE::INVALID_STATE) {
     state = state_transitions[(int)state][(int)event];
 
-    if (state == STATE::DEPLOYING_CHUTE || state == STATE::EJECTION_TEST_EJECT) {
+    if (state == STATE::DEPLOYING_DROGUE || state == STATE::EJECTION_TEST_EJECT) {
       ejection_start = millis();
     }
 
+    if (state == STATE::DEPLOYING_MAIN) {
+      main_deployment_start = millis();
+    }
+
     if (event == EVENT::LAUNCHED) {
       onLaunched();
     }
@@ -114,9 +131,13 @@ void FSM::onSetup() {
   gps->setup();
   telemetry->send("GPS setup complete.");
 
-  telemetry->send("Setting up Igniter..");
-  igniter->setup();
-  telemetry->send("Igniter setup complete.");
+  telemetry->send("Setting up Drogue Igniter..");
+  drogueIgniter->setup();
+  telemetry->send("Drogue Igniter setup complete.");
+
+  telemetry->send("Setting up Main Igniter..");
+  mainIgniter->setup();
+  telemetry->send("Main Igniter setup complete.");
 
   process_event(EVENT::SETUP_COMPLETE);
 }
@@ -136,6 +157,9 @@ void FSM::onCalibration() {
 }
 
 void FSM::onReady() {
+  telemetry->setRadioThrottle(0);
+  *loop_frequency = 10;
+
   if (altimeter->agl() > LAUNCH_AGL_THRESHOLD or
       (imu_sensor->accelerationX() / GRAVITY) * -1 > LAUNCH_ACCELERATION_THRESHOLD) {
     process_event(EVENT::LAUNCHED);
@@ -147,7 +171,7 @@ void FSM::onEjectionTestReady() {}
 void FSM::onEjectionTestEject() {
   telemetry->setRadioThrottle(1000);
   *loop_frequency = 100;
-  onDeployingChute();
+  onDeployingDrogue();
 }
 
 void FSM::onEjectionTestComplete() {
@@ -174,11 +198,34 @@ void FSM::onApogeeTimeout() {
   process_event(EVENT::APOGEE_DETECTED);
 }
 
-void FSM::onDeployingChute() {
-  igniter->enable();
-  if (millis() - ejection_start > EJECTION_TIMEOUT) {
-    igniter->disable();
-    process_event(EVENT::CHUTE_EJECTED);
+void FSM::onDeployingDrogue() {
+  drogueIgniter->enable();
+  if (millis() - ejection_start > PYRO_TIMEOUT) {
+    drogueIgniter->disable();
+    drogue_deployment_time = millis();
+    process_event(EVENT::DROGUE_DEPLOYED);
+    telemetry->setRadioThrottle(0);
+    *loop_frequency = 10;
+  }
+}
+
+void FSM::onWaitingForMain() {
+  float agl = altimeter->agl();
+  if (agl <= MAIN_CHUTE_ALTITUDE) {
+    process_event(EVENT::REACHED_MAIN_CHUTE_ALTITUDE);
+    return;
+  }
+
+  if (millis() - drogue_deployment_time >= MAIN_CHUTE_TIMEOUT * 1000) {
+    process_event(EVENT::MAIN_CHUTE_TIMER_TIMEOUT);
+  }
+}
+
+void FSM::onDeployingMain() {
+  mainIgniter->enable();
+  if (millis() - main_deployment_start > PYRO_TIMEOUT) {
+    mainIgniter->disable();
+    process_event(EVENT::MAIN_DEPLOYED);
     telemetry->setRadioThrottle(0);
     *loop_frequency = 10;
   }
@@ -209,7 +256,6 @@ void FSM::runCurrentState() {
 #else
   message.sd_logs_enabled = false;
 #endif
-  message.selected_igniter = IGNITER;
 
   if (state != STATE::SETUP and state != STATE::IDLE and state != STATE::CALIBRATION) {
     payload.agl_cm = altimeter->aglCM();
@@ -259,8 +305,14 @@ void FSM::runCurrentState() {
     case STATE::APOGEE_TIMEOUT:
       onApogeeTimeout();
       break;
-    case STATE::DEPLOYING_CHUTE:
-      onDeployingChute();
+    case STATE::DEPLOYING_DROGUE:
+      onDeployingDrogue();
+      break;
+    case STATE::WAITING_FOR_MAIN:
+      onWaitingForMain();
+      break;
+    case STATE::DEPLOYING_MAIN:
+      onDeployingMain();
       break;
     case STATE::RECOVERING:
       onRecovering();

libraries/fsm.h

@@ -26,7 +26,7 @@ class FSM {
     void process_event(EVENT event);
     void runCurrentState();
 
-    FSM(Telemetry* telemetry, IMU* imu_sensor, Altimeter* altimeter, GPSReceiver* gps, Igniter* igniter, uint8_t* loop_frequency);
+    FSM(Telemetry* telemetry, IMU* imu_sensor, Altimeter* altimeter, GPSReceiver* gps, Igniter* drogueIgniter, Igniter* mainIgniter, uint8_t* loop_frequency);
 
   private:
     STATE state = STATE::SETUP;
@@ -36,11 +36,14 @@ class FSM {
     IMU* imu_sensor;
     Altimeter* altimeter;
     GPSReceiver* gps;
-    Igniter* igniter;
+    Igniter* drogueIgniter;
+    Igniter* mainIgniter;
     uint8_t* loop_frequency;
 
     unsigned long launch_time;
     unsigned long ejection_start;
+    unsigned long main_deployment_start;
+    unsigned long drogue_deployment_time;
     float max_agl = 0;
 
     void onSetup();
@@ -53,6 +56,8 @@ class FSM {
     void onEjectionTestComplete();
     void onAscending();
     void onApogeeTimeout();
-    void onDeployingChute();
+    void onDeployingDrogue();
+    void onWaitingForMain();
+    void onDeployingMain();
     void onRecovering();
 };

libraries/mosfet_igniter.cpp

@@ -11,29 +11,24 @@ IgniterOptions igniters[2] = {
   {5, 16}, {6, 15}
 };
 
-MosfetIgniter& MosfetIgniter::getInstance() {
-  static MosfetIgniter instance;
-  return instance;
-}
-
 void MosfetIgniter::setup() {
-  pinMode(igniters[IGNITER - 1].mosfet_pin, OUTPUT);
+  pinMode(igniters[igniterChannel - 1].mosfet_pin, OUTPUT);
 #if IGN_DEBUG
-  pinMode(igniters[IGNITER - 1].led_pin, OUTPUT);
+  pinMode(igniters[igniterChannel - 1].led_pin, OUTPUT);
 #endif
-  digitalWrite(igniters[IGNITER - 1].mosfet_pin, LOW);
+  digitalWrite(igniters[igniterChannel - 1].mosfet_pin, LOW);
 }
 
 void MosfetIgniter::enable() {
-  digitalWrite(igniters[IGNITER - 1].mosfet_pin, HIGH);
+  digitalWrite(igniters[igniterChannel - 1].mosfet_pin, HIGH);
 #if IGN_DEBUG
-  digitalWrite(igniters[IGNITER - 1].led_pin, HIGH);
+  digitalWrite(igniters[igniterChannel - 1].led_pin, HIGH);
 #endif
 }
 
 void MosfetIgniter::disable() {
-  digitalWrite(igniters[IGNITER - 1].mosfet_pin, LOW);
+  digitalWrite(igniters[igniterChannel - 1].mosfet_pin, LOW);
 #if IGN_DEBUG
-  digitalWrite(igniters[IGNITER - 1].led_pin, LOW);
+  digitalWrite(igniters[igniterChannel - 1].led_pin, LOW);
 #endif
 }

libraries/mosfet_igniter.h

@@ -1,9 +1,15 @@
 #include "igniter.h"
 
 class MosfetIgniter: public Igniter {
+  private:
+    int igniterChannel;
   public:
-    static MosfetIgniter& getInstance();
+    MosfetIgniter(int channel);
     void setup();
     void enable();
     void disable();
 };
+
+inline MosfetIgniter::MosfetIgniter(int channel)
+  : igniterChannel(channel)
+{}

libraries/types.cpp

@@ -17,7 +17,9 @@ static String StateNames[] = {
   "EJECTION_TEST_COMPLETE",
   "ASCENDING",
   "APOGEE_TIMEOUT",
-  "DEPLOYING_CHUTE",
+  "DEPLOYING_DROGUE",
+  "WAITING_FOR_MAIN",
+  "DEPLOYING_MAIN",
   "RECOVERING"
 };
 
@@ -35,7 +37,10 @@ static String EventNames[] = {
   "APOGEE_TIMER_TIMEOUT",
   "APOGEE_DETECTED",
   "TRIGGER_FTS",
-  "CHUTE_EJECTED",
+  "DROGUE_DEPLOYED",
+  "MAIN_CHUTE_TIMER_TIMEOUT",
+  "REACHED_MAIN_CHUTE_ALTITUDE",
+  "MAIN_DEPLOYED",
   "GO_IDLE"
 };
 
@@ -72,8 +77,7 @@ String stringifyTelemetryMessage(TelemetryMessage message) {
       String(message.free_memory_kb) + "," +
       String(message.battery_voltage_mv) + "," +
       String(state_to_str(message.state)) + "," +
-      String(message.sd_logs_enabled) + "," +
-      String(message.selected_igniter);
+      String(message.sd_logs_enabled);
 
     if (message.state != STATE::SETUP and message.state != STATE::IDLE and message.state != STATE::CALIBRATION) {
       message_str += "," +

libraries/types.h

@@ -21,7 +21,9 @@ enum class STATE : uint8_t {
   EJECTION_TEST_COMPLETE, /*!< */
   ASCENDING, /*!< The rocket has detected that it is ascending. */
   APOGEE_TIMEOUT, /*!< */
-  DEPLOYING_CHUTE, /*!< */
+  DEPLOYING_DROGUE, /*!< */
+  WAITING_FOR_MAIN, /*!< */
+  DEPLOYING_MAIN, /*!< */
   RECOVERING, /*!< The recovery process of the rocket has began. */
   Count /*!< */
 };
@@ -35,7 +37,10 @@ enum class EVENT : uint8_t {
   APOGEE_TIMER_TIMEOUT, /*!< */
   APOGEE_DETECTED, /*!< */
   TRIGGER_FTS, /*!< */
-  CHUTE_EJECTED, /*!< */
+  DROGUE_DEPLOYED, /*!< */
+  MAIN_CHUTE_TIMER_TIMEOUT, /*!< */
+  REACHED_MAIN_CHUTE_ALTITUDE, /*!< */
+  MAIN_DEPLOYED, /*!< */
   GO_IDLE, /*!< */
   Count /*!< */
 };
@@ -121,7 +126,6 @@ struct TelemetryMessage {
   uint16_t battery_voltage_mv; /*!< Voltage of the battery. Usefull in determining battery percentage. */
   STATE state; /*!< State of the microcontroller and the rocket in general. */
   bool sd_logs_enabled; /*!< True if the logging output will be saved to the SD as well. */
-  uint8_t selected_igniter; /*!< */
   TelemetryMessagePayload payload; /*!< Data returned from the sensors. */
   char debug_message[80]; /*!< Debug message c style string. */
 };