diff --git a/flight-computer.ino b/flight-computer.ino index 48af264..ade4073 100644 --- a/flight-computer.ino +++ b/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); diff --git a/libraries/definitions.h b/libraries/definitions.h index 183dc2a..f8480ec 100644 --- a/libraries/definitions.h +++ b/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 diff --git a/libraries/fsm.cpp b/libraries/fsm.cpp index 1d7dc67..cfb7815 100644 --- a/libraries/fsm.cpp +++ b/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(); diff --git a/libraries/fsm.h b/libraries/fsm.h index 7c23b5d..745eeb7 100644 --- a/libraries/fsm.h +++ b/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(); }; diff --git a/libraries/mosfet_igniter.cpp b/libraries/mosfet_igniter.cpp index 941531f..b821b1e 100644 --- a/libraries/mosfet_igniter.cpp +++ b/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 } diff --git a/libraries/mosfet_igniter.h b/libraries/mosfet_igniter.h index 185296b..48adeb2 100644 --- a/libraries/mosfet_igniter.h +++ b/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) +{} \ No newline at end of file diff --git a/libraries/types.cpp b/libraries/types.cpp index 0071982..7a1dfb4 100644 --- a/libraries/types.cpp +++ b/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 += "," + diff --git a/libraries/types.h b/libraries/types.h index c937b5e..710fc52 100644 --- a/libraries/types.h +++ b/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. */ };