diff --git a/src/boards/Main/Configs/FlightModeManagerConfig.h b/src/boards/Main/Configs/FlightModeManagerConfig.h index fbe7a40ba19044dd3d4ad712ccbdea792c1c49ec..857df4abf2affe57a0520e035ec3298483de59b0 100644 --- a/src/boards/Main/Configs/FlightModeManagerConfig.h +++ b/src/boards/Main/Configs/FlightModeManagerConfig.h @@ -26,5 +26,6 @@ namespace Main { constexpr unsigned int MISSION_TIMEOUT = 15 * 60 * 1000; +constexpr unsigned int LANDING_TIMEOUT = 2 * 60 * 1000; -} +} // namespace Main diff --git a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp index e9d6d70e8027728d47fe8116a5b0d692527a610c..a0fe06d3251890d119a01a2a3128776227d649a6 100644 --- a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp +++ b/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp @@ -81,6 +81,7 @@ void FlightStatsRecorder::update(Boardcore::ADAState state) (state.x0 < stats.ada_min_pressure || stats.ada_min_pressure == -1)) { stats.ada_min_pressure = state.x0; + stats.apogee_alt = state.aglAltitude; } } @@ -112,7 +113,9 @@ void FlightStatsRecorder::update(PressureData data) if ((fmmState.state == FlightModeManagerState::ASCENDING || fmmState.state == FlightModeManagerState::DROGUE_DESCENT) && (data.pressure < stats.min_pressure || stats.min_pressure == -1)) + { stats.min_pressure = data.pressure; + } } void FlightStatsRecorder::updateDplVane(PressureData data) @@ -128,7 +131,6 @@ void FlightStatsRecorder::updateDplVane(PressureData data) void FlightStatsRecorder::setApogee(GPSData data) { - stats.apogee_alt = data.height; stats.apogee_lat = data.latitude; stats.apogee_lon = data.longitude; } diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp index c7a7418f548d8387b6c01dc1675832910517a38c..039f389108b1deb058b2bdf1ec802a016f4da352 100644 --- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp +++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp @@ -25,6 +25,7 @@ #include <Main/BoardScheduler.h> #include <Main/Configs/AirBrakesControllerConfig.h> #include <Main/Configs/NASConfig.h> +#include <Main/FlightStatsRecorder/FlightStatsRecorder.h> #include <Main/Sensors/Sensors.h> #include <Main/StateMachines/AirBrakesController/AirBrakesController.h> #include <common/ReferenceConfig.h> @@ -209,6 +210,7 @@ void ADAController::update() } Logger::getInstance().log(ada.getState()); + FlightStatsRecorder::getInstance().update(ada.getState()); #ifdef HILSimulation // useful only for hil testing diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp index 326f26e3295768d6db9ba9129a4fca6449433024..878d4145862e8bf19f5baff40068a9f67481bf01 100644 --- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp +++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp @@ -23,7 +23,9 @@ #include "FlightModeManager.h" #include <Main/Actuators/Actuators.h> +#include <Main/CanHandler/CanHandler.h> #include <Main/Configs/FlightModeManagerConfig.h> +#include <Main/FlightStatsRecorder/FlightStatsRecorder.h> #include <Main/Sensors/Sensors.h> #include <common/events/Events.h> #include <drivers/timer/TimestampTimer.h> @@ -73,6 +75,7 @@ State FlightModeManager::state_on_ground(const Event& event) } case TMTC_RESET_BOARD: { + CanHandler::getInstance().sendCamOffCommand(); Logger::getInstance().stop(); reboot(); return HANDLED; @@ -126,8 +129,8 @@ State FlightModeManager::state_init_error(const Event& event) { case EV_ENTRY: { - Actuators::getInstance().buzzerError(); logStatus(FlightModeManagerState::INIT_ERROR); + Actuators::getInstance().buzzerError(); EventBroker::getInstance().post(FLIGHT_ERROR_DETECTED, TOPIC_FLIGHT); return HANDLED; @@ -252,6 +255,7 @@ State FlightModeManager::state_disarmed(const Event& event) case EV_ENTRY: { logStatus(FlightModeManagerState::DISARMED); + Actuators::getInstance().buzzerDisarmed(); Logger::getInstance().stop(); EventBroker::getInstance().post(FLIGHT_DISARMED, TOPIC_FLIGHT); @@ -338,6 +342,7 @@ State FlightModeManager::state_armed(const Event& event) case EV_ENTRY: { logStatus(FlightModeManagerState::ARMED); + Actuators::getInstance().buzzerArmed(); Logger::getInstance().start(); EventBroker::getInstance().post(FLIGHT_ARMED, TOPIC_FLIGHT); @@ -412,7 +417,7 @@ State FlightModeManager::state_flying(const Event& event) } case FLIGHT_MISSION_TIMEOUT: { - return transition(&FlightModeManager::state_landed); + return transition(&FlightModeManager::state_mission_ended); } default: { @@ -447,6 +452,8 @@ State FlightModeManager::state_ascending(const Event& event) case TMTC_FORCE_APOGEE: case TMTC_FORCE_EXPULSION: { + FlightStatsRecorder::getInstance().setApogee( + Sensors::getInstance().getUbxGpsLastSample()); return transition(&FlightModeManager::state_drogue_descent); } default: @@ -535,12 +542,58 @@ State FlightModeManager::state_terminal_descent(const Event& event) State FlightModeManager::state_landed(const Event& event) { + static uint16_t landingTimeoutEventId = -1; + switch (event) { case EV_ENTRY: { logStatus(FlightModeManagerState::LANDED); + landingTimeoutEventId = + EventBroker::getInstance().postDelayed<LANDING_TIMEOUT>( + FLIGHT_LANDING_TIMEOUT, TOPIC_FLIGHT); + + Actuators::getInstance().buzzerLanded(); + + return HANDLED; + } + case EV_EXIT: + { + EventBroker::getInstance().removeDelayed(landingTimeoutEventId); + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_top); + } + case EV_INIT: + { + return HANDLED; + } + case FLIGHT_LANDING_TIMEOUT: + { + return transition(&FlightModeManager::state_mission_ended); + } + case TMTC_RESET_BOARD: + { + CanHandler::getInstance().sendCamOffCommand(); + Logger::getInstance().stop(); + reboot(); + return HANDLED; + } + default: + { + return UNHANDLED; + } + } +} +State FlightModeManager::state_mission_ended(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { Actuators::getInstance().buzzerLanded(); Logger::getInstance().stop(); @@ -560,6 +613,7 @@ State FlightModeManager::state_landed(const Event& event) } case TMTC_RESET_BOARD: { + CanHandler::getInstance().sendCamOffCommand(); Logger::getInstance().stop(); reboot(); return HANDLED; diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h index aa21d1566e4f8a58e19aa4415881e78eccd94dac..1e9449fba6caa4eca58012a653d843bc649c4d85 100644 --- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h +++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h @@ -78,6 +78,9 @@ public: /// The rocket is on the ground after the flight. Boardcore::State state_landed(const Boardcore::Event& event); + /// The rocket ended the flight and closes the log. + Boardcore::State state_mission_ended(const Boardcore::Event& event); + private: FlightModeManager(); ~FlightModeManager(); diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h index 9081054d750b8332559e7256d0a20745c23ca6c3..f0fa03db56a9bfcac59c6ab4f85d39475881e018 100644 --- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h +++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h @@ -43,7 +43,8 @@ enum class FlightModeManagerState : uint8_t ASCENDING, DROGUE_DESCENT, TERMINAL_DESCENT, - LANDED + LANDED, + MISSION_ENDED, }; struct FlightModeManagerStatus diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp index bd5d9bcf804fa600ea658c97991d340c5b431583..04f71543ecf2b0be6afaf8cb425d83e6f2614dc7 100644 --- a/src/boards/Main/StateMachines/NASController/NASController.cpp +++ b/src/boards/Main/StateMachines/NASController/NASController.cpp @@ -24,6 +24,7 @@ #include <Main/BoardScheduler.h> #include <Main/Configs/NASConfig.h> +#include <Main/FlightStatsRecorder/FlightStatsRecorder.h> #include <Main/Sensors/Sensors.h> #include <Main/StateMachines/FlightModeManager/FlightModeManager.h> #include <algorithms/NAS/StateInitializer.h> @@ -76,6 +77,7 @@ void NASController::update() nas.correctPitot(pitotData.deltaP, pressureData.pressure); Logger::getInstance().log(nas.getState()); + FlightStatsRecorder::getInstance().update(nas.getState()); } #ifdef HILSimulation diff --git a/src/boards/Payload/Configs/FlightModeManagerConfig.h b/src/boards/Payload/Configs/FlightModeManagerConfig.h index 95462eb059799913265967f5f4bd0f9fa7b2328a..d36b0601d8725248fe7b943dffaf2e50cea18576 100644 --- a/src/boards/Payload/Configs/FlightModeManagerConfig.h +++ b/src/boards/Payload/Configs/FlightModeManagerConfig.h @@ -25,7 +25,8 @@ namespace Payload { -constexpr unsigned int MISSION_TIMEOUT = 15 * 60 * 1000; constexpr unsigned int APOGEE_TIMEOUT = 20 * 1000; +constexpr unsigned int MISSION_TIMEOUT = 15 * 60 * 1000; +constexpr unsigned int LANDING_TIMEOUT = 2 * 60 * 1000; } // namespace Payload diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp index 9253e42d9cf6873822e69b00308bfc46c38a7fa2..8e2591c63dc8b8f85e7d67513ecc3d9dc53a9d4e 100644 --- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp +++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp @@ -84,6 +84,7 @@ State FlightModeManager::state_on_ground(const Event& event) } case TMTC_RESET_BOARD: { + Actuators::getInstance().camOff(); Logger::getInstance().stop(); reboot(); return HANDLED; @@ -247,8 +248,8 @@ State FlightModeManager::state_disarmed(const Event& event) logStatus(FlightModeManagerState::DISARMED); Actuators::getInstance().ledDisarmed(); - Logger::getInstance().stop(); Actuators::getInstance().camOff(); + Logger::getInstance().stop(); EventBroker::getInstance().post(FLIGHT_DISARMED, TOPIC_FLIGHT); return HANDLED; @@ -313,11 +314,6 @@ State FlightModeManager::state_test_mode(const Event& event) { return HANDLED; } - case TMTC_FORCE_MAIN: - { - EventBroker::getInstance().post(DPL_CUT_DROGUE, TOPIC_DPL); - return HANDLED; - } case TMTC_EXIT_TEST_MODE: { return transition(&FlightModeManager::state_disarmed); @@ -338,8 +334,8 @@ State FlightModeManager::state_armed(const Event& event) logStatus(FlightModeManagerState::ARMED); Actuators::getInstance().ledArmed(); - Logger::getInstance().start(); Actuators::getInstance().camOn(); + Logger::getInstance().start(); EventBroker::getInstance().post(FLIGHT_ARMED, TOPIC_FLIGHT); return HANDLED; @@ -407,9 +403,8 @@ State FlightModeManager::state_flying(const Event& event) case FLIGHT_MISSION_TIMEOUT: { WingController::getInstance().stop(); - Actuators::getInstance().camOff(); - return transition(&FlightModeManager::state_landed); + return transition(&FlightModeManager::state_mission_ended); } default: { @@ -543,12 +538,57 @@ State FlightModeManager::state_terminal_descent(const Event& event) State FlightModeManager::state_landed(const Event& event) { + static uint16_t landingTimeoutEventId = -1; + switch (event) { case EV_ENTRY: { logStatus(FlightModeManagerState::LANDED); + landingTimeoutEventId = + EventBroker::getInstance().postDelayed<LANDING_TIMEOUT>( + FLIGHT_LANDING_TIMEOUT, TOPIC_FLIGHT); + + return HANDLED; + } + case EV_EXIT: + { + EventBroker::getInstance().removeDelayed(landingTimeoutEventId); + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_top); + } + case EV_INIT: + { + return HANDLED; + } + case FLIGHT_LANDING_TIMEOUT: + { + return transition(&FlightModeManager::state_mission_ended); + } + case TMTC_RESET_BOARD: + { + Actuators::getInstance().camOff(); + Logger::getInstance().stop(); + reboot(); + return HANDLED; + } + default: + { + return UNHANDLED; + } + } +} +State FlightModeManager::state_mission_ended(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + Actuators::getInstance().camOff(); Logger::getInstance().stop(); return HANDLED; @@ -567,6 +607,7 @@ State FlightModeManager::state_landed(const Event& event) } case TMTC_RESET_BOARD: { + Actuators::getInstance().camOff(); Logger::getInstance().stop(); reboot(); return HANDLED; diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h index a79a4031b2a10c7a98d98dd76d37bbd148ccb74f..f5bfaeef4d23c33a88b26d94839ce288f425ca0f 100644 --- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h +++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h @@ -77,6 +77,9 @@ public: /// The payload is on the ground after the flight. Boardcore::State state_landed(const Boardcore::Event& event); + /// The rocket ended the flight and closes the log. + Boardcore::State state_mission_ended(const Boardcore::Event& event); + private: FlightModeManager(); ~FlightModeManager(); diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManagerData.h index e46b8c158f525e7a28eb1b17c1f43e35fd2e17ef..f5ec50adc4af973b6fea01728857e1e7986e9b8e 100644 --- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManagerData.h +++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManagerData.h @@ -43,7 +43,8 @@ enum class FlightModeManagerState : uint8_t ASCENDING, DROGUE_DESCENT, TERMINAL_DESCENT, - LANDED + LANDED, + MISSION_ENDED, }; struct FlightModeManagerStatus diff --git a/src/boards/common/events/Events.h b/src/boards/common/events/Events.h index 297ccbdf9905544c53b2c3a71021408c495cd969..f9a031a8ea020e50ba86ff9d9d12c9222ac67468 100644 --- a/src/boards/common/events/Events.h +++ b/src/boards/common/events/Events.h @@ -59,6 +59,7 @@ enum Events : uint8_t FLIGHT_DPL_ALT_DETECTED, FLIGHT_ERROR_DETECTED, FLIGHT_LANDING_DETECTED, + FLIGHT_LANDING_TIMEOUT, FLIGHT_LAUNCH_PIN_DETACHED, FLIGHT_LIFTOFF, FLIGHT_MISSION_TIMEOUT,