diff --git a/src/Parafoil/BoardScheduler.h b/src/Parafoil/BoardScheduler.h index 408d758100578da03dfa690f6f2bfd248cd0328a..8f89928081b6b4dfc5c2594e8cc9c52dab05b779 100644 --- a/src/Parafoil/BoardScheduler.h +++ b/src/Parafoil/BoardScheduler.h @@ -25,7 +25,7 @@ #include <scheduler/TaskScheduler.h> #include <utils/DependencyManager/DependencyManager.h> -namespace Payload +namespace Parafoil { /** @@ -52,6 +52,21 @@ public: }; }; + static Priority::PriorityLevel flightModeManagerPriority() + { + return Priority::MEDIUM; + } + + static Priority::PriorityLevel nasControllerPriority() + { + return Priority::MEDIUM; + } + + static Priority::PriorityLevel wingControllerPriority() + { + return Priority::MEDIUM; + } + /** * @brief Starts all the schedulers */ @@ -102,4 +117,4 @@ private: Boardcore::Logging::getLogger("BoardScheduler"); }; -} // namespace Payload +} // namespace Parafoil diff --git a/src/Parafoil/Configs/FlightModeManagerConfig.h b/src/Parafoil/Configs/FlightModeManagerConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..2aed44f469f3af11af476dcdd1ea02c0a462378c --- /dev/null +++ b/src/Parafoil/Configs/FlightModeManagerConfig.h @@ -0,0 +1,41 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <chrono> + +namespace Parafoil +{ +namespace Config +{ +namespace FlightModeManager +{ + +/* linter-off */ using namespace std::chrono_literals; + +constexpr auto LOGGING_DELAY = 5s; +constexpr auto CONTROL_DELAY = 5s; + +} // namespace FlightModeManager +} // namespace Config +} // namespace Parafoil diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a3b781bddbb8ba0f0d775095e1374ae7d78799b0 --- /dev/null +++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp @@ -0,0 +1,545 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "FlightModeManager.h" + +#include <Parafoil/BoardScheduler.h> +#include <Parafoil/Configs/FlightModeManagerConfig.h> +#include <common/Events.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> + +using namespace Boardcore; +using namespace Common; +using namespace std::chrono; +namespace config = Parafoil::Config::FlightModeManager; + +namespace Parafoil +{ + +FlightModeManager::FlightModeManager() + : HSM(&FlightModeManager::PreFlight, miosix::STACK_DEFAULT_FOR_PTHREAD, + BoardScheduler::flightModeManagerPriority()) +{ + EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT); + EventBroker::getInstance().subscribe(this, TOPIC_FMM); + EventBroker::getInstance().subscribe(this, TOPIC_TMTC); + EventBroker::getInstance().subscribe(this, TOPIC_NAS); +} + +FlightModeManager::~FlightModeManager() +{ + EventBroker::getInstance().unsubscribe(this); +} + +FlightModeManagerState FlightModeManager::getState() { return state; } + +bool FlightModeManager::isTestMode() const +{ + return state == FlightModeManagerState::READY_TEST_MODE; +} + +State FlightModeManager::PreFlight(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return HANDLED; + } + + case EV_EXIT: + { + return HANDLED; + } + + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_top); + } + + case EV_INIT: + { + return transition(&FlightModeManager::PreFlightInit); + } + + case TMTC_START_LOGGING: + { + Logger::getInstance().start(); + return HANDLED; + } + + case TMTC_STOP_LOGGING: + { + Logger::getInstance().stop(); + return HANDLED; + } + + case TMTC_RESET_BOARD: + { + Logger::getInstance().stop(); + miosix::reboot(); + __builtin_unreachable(); + } + + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::PreFlightInit(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(FlightModeManagerState::PRE_FLIGHT_INIT); + return HANDLED; + } + + case EV_EXIT: + { + return HANDLED; + } + + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::PreFlight); + } + + case EV_INIT: + { + return HANDLED; + } + + case FMM_INIT_OK: + { + return transition(&FlightModeManager::PreFlightInitDone); + } + + case FMM_INIT_ERROR: + { + return transition(&FlightModeManager::PreFlightInitError); + } + + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::PreFlightInitError(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(FlightModeManagerState::PRE_FLIGHT_INIT_ERROR); + return HANDLED; + } + + case EV_EXIT: + { + return HANDLED; + } + + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::PreFlight); + } + + case EV_INIT: + { + return HANDLED; + } + + case TMTC_FORCE_INIT: + { + return transition(&FlightModeManager::PreFlightInitDone); + } + + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::PreFlightInitDone(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(FlightModeManagerState::PRE_FLIGHT_INIT_DONE); + EventBroker::getInstance().post(FMM_CALIBRATE, TOPIC_FMM); + return HANDLED; + } + + case EV_EXIT: + { + return HANDLED; + } + + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::PreFlight); + } + + case EV_INIT: + { + return HANDLED; + } + + case FMM_CALIBRATE: + { + return transition(&FlightModeManager::PreFlightSensorCalibration); + } + + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::PreFlightSensorCalibration(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState( + FlightModeManagerState::PRE_FLIGHT_ALGORITHM_CALIBRATION); + + // Wait for sensors to stabilize before calibration + // The first few LPS28DFW samples contain garbage data + miosix::Thread::sleep(100); + getModule<Sensors>()->calibrate(); + + EventBroker::getInstance().post(FMM_ALGOS_CALIBRATE, TOPIC_FMM); + return HANDLED; + } + + case EV_EXIT: + { + return HANDLED; + } + + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::PreFlight); + } + + case EV_INIT: + { + return HANDLED; + } + + case FMM_ALGOS_CALIBRATE: + { + return transition( + &FlightModeManager::PreFlightAlgorithmCalibration); + } + + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::PreFlightAlgorithmCalibration(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState( + FlightModeManagerState::PRE_FLIGHT_ALGORITHM_CALIBRATION); + // Calibrate after a delay to allow calibrated sensors to stabilize + EventBroker::getInstance().postDelayed(NAS_CALIBRATE, TOPIC_NAS, + 100); + return HANDLED; + } + + case EV_EXIT: + { + return HANDLED; + } + + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::PreFlight); + } + + case EV_INIT: + { + return HANDLED; + } + + case NAS_READY: + { + EventBroker::getInstance().post(FMM_READY, TOPIC_FMM); + return transition(&FlightModeManager::Ready); + } + + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::Ready(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(FlightModeManagerState::READY); + return HANDLED; + } + + case EV_EXIT: + { + return HANDLED; + } + + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_top); + } + + case EV_INIT: + { + return HANDLED; + } + + case TMTC_ENTER_TEST_MODE: + { + return transition(&FlightModeManager::ReadyTestMode); + } + + case TMTC_CALIBRATE: + { + return transition(&FlightModeManager::PreFlightSensorCalibration); + } + + case FLIGHT_NC_DETACHED: + case TMTC_FORCE_EXPULSION: + { + return transition(&FlightModeManager::FlyingWingDescent); + } + + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::ReadyTestMode(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(FlightModeManagerState::READY_TEST_MODE); + return HANDLED; + } + + case EV_EXIT: + { + return HANDLED; + } + + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::Ready); + } + + case EV_INIT: + { + return HANDLED; + } + + case TMTC_FORCE_LANDING: + { + return transition(&FlightModeManager::Landed); + } + + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::Flying(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + return HANDLED; + } + + case EV_EXIT: + { + return HANDLED; + } + + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_top); + } + + case EV_INIT: + { + return transition(&FlightModeManager::FlyingWingDescent); + } + + case TMTC_FORCE_LANDING: + case FLIGHT_MISSION_TIMEOUT: + { + return transition(&FlightModeManager::Landed); + } + + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::FlyingWingDescent(const Event& event) +{ + static uint16_t controlDelayId; + + switch (event) + { + case EV_ENTRY: + { + updateState(FlightModeManagerState::FLYING_WING_DESCENT); + // Send the event to the WingController + controlDelayId = EventBroker::getInstance().postDelayed( + FLIGHT_WING_DESCENT, TOPIC_FLIGHT, + milliseconds{config::CONTROL_DELAY}.count()); + return HANDLED; + } + + case EV_EXIT: + { + EventBroker::getInstance().removeDelayed(controlDelayId); + return HANDLED; + } + + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::Flying); + } + + case EV_INIT: + { + return HANDLED; + } + + case TMTC_FORCE_LANDING: + { + return transition(&FlightModeManager::Landed); + } + + default: + { + return UNHANDLED; + } + } +} + +State FlightModeManager::Landed(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(FlightModeManagerState::LANDED); + + EventBroker::getInstance().post(FLIGHT_LANDING_DETECTED, + TOPIC_FLIGHT); + EventBroker::getInstance().postDelayed( + FMM_STOP_LOGGING, TOPIC_FMM, + milliseconds{config::LOGGING_DELAY}.count()); + + return HANDLED; + } + + case EV_EXIT: + { + return HANDLED; + } + + case EV_EMPTY: + { + return tranSuper(&FlightModeManager::state_top); + } + + case EV_INIT: + { + return HANDLED; + } + + case FMM_STOP_LOGGING: + { + Logger::getInstance().stop(); + return HANDLED; + } + + case TMTC_RESET_BOARD: + { + Logger::getInstance().stop(); + miosix::reboot(); + __builtin_unreachable(); + } + + default: + { + return UNHANDLED; + } + } +} + +void FlightModeManager::updateState(FlightModeManagerState newState) +{ + state = newState; + + auto status = FlightModeManagerStatus{ + .timestamp = TimestampTimer::getTimestamp(), + .state = newState, + }; + Logger::getInstance().log(status); +} + +} // namespace Parafoil \ No newline at end of file diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h new file mode 100644 index 0000000000000000000000000000000000000000..28b98b68c4977d3530fd33f7a7785a97ee17852f --- /dev/null +++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h @@ -0,0 +1,158 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <events/HSM.h> +#include <utils/DependencyManager/DependencyManager.h> + +#include "FlightModeManagerData.h" + +namespace Parafoil +{ +class Sensors; +class CanHandler; +class Actuators; +class AltitudeTrigger; +class FlightStatsRecorder; +class NASController; + +/** + * State machine that manages the flight modes of the Parafoil. + * + * HSM Schema: + * + * PreFlight + * ├── PreFlightInit + * ├── PreFlightInitError + * ├── PreFlightInitDone + * ├── PreFlightSensorCalibration + * └── PreFlightAlgorithmCalibration + * + * Ready + * └── ReadyTestMode + * + * Flying + * ├── FlyingDrogueDescent + * └── FlyingWingDescent + * + * Landed + */ +class FlightModeManager + : public Boardcore::HSM<FlightModeManager>, + public Boardcore::InjectableWithDeps<Sensors, CanHandler, Actuators, + AltitudeTrigger, FlightStatsRecorder, + NASController> +{ + FlightModeManager(); + ~FlightModeManager(); + + /** + * @brief Returns the current state of the FlightModeManager. + */ + FlightModeManagerState getState(); + + bool isTestMode() const; + + /** + * @brief Super state for when the parafoil has not yet ready for launch. + */ + Boardcore::State PreFlight(const Boardcore::Event& event); + + /** + * @brief State in which the parafoil is initializing. + * + * Super state: PreFlight + */ + Boardcore::State PreFlightInit(const Boardcore::Event& event); + + /** + * @brief State in which the init has failed + * + * Super state: PreFlight + */ + Boardcore::State PreFlightInitError(const Boardcore::Event& event); + + /** + * @brief State in which the init is done and a calibration event is + * thrown + * + * Super state: PreFlight + */ + Boardcore::State PreFlightInitDone(const Boardcore::Event& event); + + /** + * @brief Calibration of all sensors. + * + * Super state: PreFlight + */ + Boardcore::State PreFlightSensorCalibration(const Boardcore::Event& event); + + /** + * @brief Calibration of all algorithms. + * + * Super state: PreFlight + */ + Boardcore::State PreFlightAlgorithmCalibration( + const Boardcore::Event& event); + + /** + * @brief Super state in which the parafoil is waiting to be dropped. + */ + Boardcore::State Ready(const Boardcore::Event& event); + + /** + * @brief The parafoil will accept specific telecommands otherwise + * considered risky. + * + * Super state: Ready + */ + Boardcore::State ReadyTestMode(const Boardcore::Event& event); + + /** + * @brief Super state for when the Parafoil is flying. + */ + Boardcore::State Flying(const Boardcore::Event& event); + + /** + * @brief State in which the parafoil wing is opened and starts guiding + * itself + * + * Super state: Flying + */ + Boardcore::State FlyingWingDescent(const Boardcore::Event& event); + + /** + * @brief The parafoil ended the flight and closes the log. + */ + Boardcore::State Landed(const Boardcore::Event& event); + +private: + void updateState(FlightModeManagerState newState); + + std::atomic<FlightModeManagerState> state{ + FlightModeManagerState::PRE_FLIGHT}; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("FMM"); +}; + +} // namespace Parafoil \ No newline at end of file diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManagerData.h new file mode 100644 index 0000000000000000000000000000000000000000..8b180f966afd00438b856437f66a46222ac499de --- /dev/null +++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManagerData.h @@ -0,0 +1,59 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Basso + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <cstdint> +#include <ostream> +#include <string> +namespace Parafoil +{ + +enum class FlightModeManagerState : uint8_t +{ + PRE_FLIGHT = 0, + PRE_FLIGHT_INIT, + PRE_FLIGHT_INIT_ERROR, + PRE_FLIGHT_INIT_DONE, + PRE_FLIGHT_SENSOR_CALIBRATION, + PRE_FLIGHT_ALGORITHM_CALIBRATION, + PRE_FLIGHT_DISARMED, + READY, + READY_TEST_MODE, + FLYING_WING_DESCENT, + LANDED, +}; + +struct FlightModeManagerStatus +{ + uint64_t timestamp = 0; + FlightModeManagerState state = FlightModeManagerState::PRE_FLIGHT; + + static std::string header() { return "timestamp,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "\n"; + } +}; + +} // namespace Parafoil \ No newline at end of file diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp index 911a9c80b3658f7d0b458e6b662210fe3590f668..8c07feaae339f268793159054e792d909dae5144 100644 --- a/src/Parafoil/parafoil-entry.cpp +++ b/src/Parafoil/parafoil-entry.cpp @@ -20,6 +20,7 @@ * THE SOFTWARE. */ +#include <Parafoil/BoardScheduler.h> #include <Parafoil/Buses.h> #include <diagnostic/PrintLogger.h> #include <utils/DependencyManager/DependencyManager.h> @@ -51,6 +52,8 @@ int main() // Core components auto buses = new Buses(); initResult &= depman.insert(buses); + auto scheduler = new BoardScheduler(); + initResult &= depman.insert(scheduler); return 0; } \ No newline at end of file