diff --git a/.vscode/settings.json b/.vscode/settings.json index 6cfb9e8aee80e5e5242e6bacebac36e42fb3a226..4d8d0a871d87451e502c1935a23937fd26d03594 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -149,6 +149,7 @@ "GPIOF", "KALM", "kalman", + "Lolli", "magcal", "mavlink", "miosix", diff --git a/CMakeLists.txt b/CMakeLists.txt index ef5fefc8bffac8411fef9ebf713d6df2b43becf9..a6226993fd468c47d4d84482e6ebb45026d7e1d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -131,3 +131,7 @@ add_executable(test-actuators src/entrypoints/Groundstation/Automated/test-actua target_include_directories(test-actuators PRIVATE ${OBSW_INCLUDE_DIRS}) # target_compile_definitions(test-actuators PRIVATE NO_SD_LOGGING) sbs_target(test-actuators stm32f767zi_automated_antennas) + +# add_executable(test-smcontroller src/entrypoints/Groundstation/Automated/test-smcontroller.cpp ${GROUNDSTATION_COMMON}) +# target_include_directories(test-smcontroller PRIVATE ${OBSW_INCLUDE_DIRS}) +# sbs_target(test-smcontroller stm32f767zi_nucleo) diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index b4b996bff2f860d2943b11ce9dec19a09cc3e500..6597cfa5ed967307049e5009367794ab1b8893a9 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -137,6 +137,7 @@ set(GROUNDSTATION_BASE src/boards/Groundstation/Common/Ports/EthernetBase.cpp src/boards/Groundstation/Common/Radio/RadioBase.cpp src/boards/Groundstation/Common/HubBase.cpp + src/boards/Groundstation/Automated/SMController/SMController.cpp ) set(GROUNDSTATION_AUTOMATED @@ -150,4 +151,4 @@ set(GROUNDSTATION_AUTOMATED set(ANTENNAS src/boards/Groundstation/Automated/Actuators/Actuators.cpp src/boards/Groundstation/Automated/Sensors/Sensors.cpp -) \ No newline at end of file +) diff --git a/src/boards/Groundstation/Automated/SMController/SMController.cpp b/src/boards/Groundstation/Automated/SMController/SMController.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c4966d5d649bff3a2170247ee3e30779fb73ecdc --- /dev/null +++ b/src/boards/Groundstation/Automated/SMController/SMController.cpp @@ -0,0 +1,639 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * 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 "SMController.h" + +#include <common/Events.h> +#include <drivers/timer/TimestampTimer.h> + +#include "SMControllerData.h" + +using namespace Boardcore; +using namespace Common; +using namespace miosix; + +namespace Antennas +{ + +SMController::SMController() : HSM(&SMController::state_config) +{ + EventBroker::getInstance().subscribe(this, TOPIC_ARP); + EventBroker::getInstance().subscribe(this, TOPIC_TMTC); +} + +// Super state +Boardcore::State SMController::state_config(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::CONFIG); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_top); + } + case EV_INIT: + { + return transition(&SMController::state_init); + } + case TMTC_ARP_RESET_BOARD: + { + reboot(); + return HANDLED; + } + default: + { + return UNHANDLED; + } + } +} + +// Super state +Boardcore::State SMController::state_feedback(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::FEEDBACK); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_top); + } + case EV_INIT: + { + return transition(&SMController::state_armed); + } + case TMTC_ARP_DISARM: + { + return transition(&SMController::state_init_done); + } + default: + { + return UNHANDLED; + } + } +} + +// Super state +Boardcore::State SMController::state_no_feedback(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::NO_FEEDBACK); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_top); + } + case EV_INIT: + { + return transition(&SMController::state_armed_nf); + } + case TMTC_ARP_DISARM: + { + return transition(&SMController::state_insert_info); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_init(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::INIT); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_config); + } + case EV_INIT: + { + return HANDLED; + } + case ARP_INIT_OK: + { + return transition(&SMController::state_init_done); + } + case ARP_INIT_ERROR: + { + return transition(&SMController::state_init_error); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_init_error(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::INIT_ERROR); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_config); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_ARP_FORCE_NO_FEEDBACK: + { + return transition(&SMController::state_insert_info); + } + case TMTC_ARP_FORCE_INIT: + { + return transition(&SMController::state_init_done); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_init_done(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::INIT_DONE); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_config); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_ARP_FORCE_NO_FEEDBACK: + { + return transition(&SMController::state_insert_info); + } + case TMTC_ARP_ARM: + { + return transition(&SMController::state_feedback); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_insert_info(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::INSERT_INFO); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_config); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_ARP_ARM: + { + return transition(&SMController::state_no_feedback); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_armed(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::ARMED); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_feedback); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_ARP_ENTER_TEST_MODE: + { + return transition(&SMController::state_test); + } + case TMTC_ARP_CALIBRATE: + { + return transition(&SMController::state_calibrate); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_test(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::TEST); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_feedback); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_ARP_EXIT_TEST_MODE: + { + return transition(&SMController::state_armed); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_calibrate(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::CALIBRATE); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_feedback); + } + case EV_INIT: + { + return HANDLED; + } + case ARP_CAL_DONE: + { + return transition(&SMController::state_fix_antennas); + } + case TMTC_ARP_RESET_ALGORITHM: + { + return transition(&SMController::state_armed); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_fix_antennas(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::FIX_ANTENNAS); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_feedback); + } + case EV_INIT: + { + return HANDLED; + } + case ARP_FIX_ANTENNAS: + { + return transition(&SMController::state_fix_rocket); + } + case TMTC_ARP_RESET_ALGORITHM: + { + return transition(&SMController::state_armed); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_fix_rocket(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::FIX_ROCKET); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_feedback); + } + case EV_INIT: + { + return HANDLED; + } + case ARP_FIX_ROCKET: + { + return transition(&SMController::state_active); + } + case TMTC_ARP_RESET_ALGORITHM: + { + return transition(&SMController::state_armed); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_active(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::ACTIVE); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_feedback); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_ARP_RESET_ALGORITHM: + { + return transition(&SMController::state_armed); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_armed_nf(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::ARMED_NF); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_no_feedback); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_ARP_ENTER_TEST_MODE: + { + return transition(&SMController::state_test_nf); + } + case TMTC_ARP_CALIBRATE: + { + return transition(&SMController::state_fix_rocket_nf); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_test_nf(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::TEST_NF); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_no_feedback); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_ARP_EXIT_TEST_MODE: + { + return transition(&SMController::state_armed_nf); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_fix_rocket_nf( + const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::FIX_ROCKET_NF); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_no_feedback); + } + case EV_INIT: + { + return HANDLED; + } + case ARP_FIX_ROCKET: + { + return transition(&SMController::state_active_nf); + } + case TMTC_ARP_RESET_ALGORITHM: + { + return transition(&SMController::state_armed_nf); + } + default: + { + return UNHANDLED; + } + } +} + +Boardcore::State SMController::state_active_nf(const Boardcore::Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + logStatus(SMControllerState::ACTIVE_NF); + return HANDLED; + } + case EV_EXIT: + { + return HANDLED; + } + case EV_EMPTY: + { + return tranSuper(&SMController::state_no_feedback); + } + case EV_INIT: + { + return HANDLED; + } + case TMTC_ARP_RESET_ALGORITHM: + { + return transition(&SMController::state_armed_nf); + } + default: + { + return UNHANDLED; + } + } +} + +void SMController::logStatus(SMControllerState state) +{ + { + PauseKernelLock lock; + status.timestamp = TimestampTimer::getTimestamp(); + status.state = state; + } + + Logger::getInstance().log(status); +} + +} // namespace Antennas diff --git a/src/boards/Groundstation/Automated/SMController/SMController.h b/src/boards/Groundstation/Automated/SMController/SMController.h new file mode 100644 index 0000000000000000000000000000000000000000..f48e7957b86f5e62ff8cdc22b0f528344e4a3394 --- /dev/null +++ b/src/boards/Groundstation/Automated/SMController/SMController.h @@ -0,0 +1,73 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * 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/EventBroker.h> +#include <events/HSM.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +#include "SMControllerData.h" + +namespace Antennas +{ + +class SMController : public Boardcore::Module, + public Boardcore::HSM<SMController> +{ +public: + SMController(); + + // FSM States + Boardcore::State state_config(const Boardcore::Event& event); + Boardcore::State state_feedback(const Boardcore::Event& event); + Boardcore::State state_no_feedback(const Boardcore::Event& event); + Boardcore::State state_init(const Boardcore::Event& event); + Boardcore::State state_init_error(const Boardcore::Event& event); + Boardcore::State state_init_done(const Boardcore::Event& event); + Boardcore::State state_insert_info(const Boardcore::Event& event); + Boardcore::State state_armed(const Boardcore::Event& event); + Boardcore::State state_test(const Boardcore::Event& event); + Boardcore::State state_calibrate(const Boardcore::Event& event); + Boardcore::State state_fix_antennas(const Boardcore::Event& event); + Boardcore::State state_fix_rocket(const Boardcore::Event& event); + Boardcore::State state_active(const Boardcore::Event& event); + Boardcore::State state_armed_nf(const Boardcore::Event& event); + Boardcore::State state_test_nf(const Boardcore::Event& event); + Boardcore::State state_fix_rocket_nf(const Boardcore::Event& event); + Boardcore::State state_active_nf(const Boardcore::Event& event); + +private: + /** + * @brief Logs the current state of the FSM + * @param state The current FSM state + */ + void logStatus(SMControllerState state); + + SMControllerStatus status; + + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("SMController"); +}; + +} // namespace Antennas diff --git a/src/boards/Groundstation/Automated/SMController/SMControllerData.h b/src/boards/Groundstation/Automated/SMController/SMControllerData.h new file mode 100644 index 0000000000000000000000000000000000000000..98b3fa259acdf35868893b9d6d784dca04d32a3c --- /dev/null +++ b/src/boards/Groundstation/Automated/SMController/SMControllerData.h @@ -0,0 +1,79 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * 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 <stdint.h> + +#include <iostream> +#include <string> + +namespace Antennas +{ + +enum class SMControllerState : uint8_t +{ + INIT = 0, + INIT_ERROR, + INIT_DONE, + INSERT_INFO, + ARMED, + ARMED_NF, + TEST, + TEST_NF, + CALIBRATE, + FIX_ANTENNAS, + FIX_ROCKET, + FIX_ROCKET_NF, + ACTIVE, + ACTIVE_NF, + /** + * @brief macro state for configuration (init, init_error, + * init_done, state_insert_info) + */ + CONFIG, + /** + * @brief macro state for feedback (armed, test, calibrate, + * fix_antennas, fix_rocket, active) + */ + FEEDBACK, + /** + * @brief macro state for no feedback (armed_nf, test_nf, + * fix_rocket_nf, active_nf) + */ + NO_FEEDBACK +}; + +struct SMControllerStatus +{ + uint64_t timestamp; + SMControllerState state; + + static std::string header() { return "timestamp,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "\n"; + } +}; + +} // namespace Antennas diff --git a/src/boards/common/Events.h b/src/boards/common/Events.h index 44fe47db9452370fd12d975f499dd830d15d2bc1..f4224d60d8929a2c303b9109deddfd87b3519030 100644 --- a/src/boards/common/Events.h +++ b/src/boards/common/Events.h @@ -55,6 +55,11 @@ enum Events : uint8_t MEA_FORCE_STOP, MEA_SHADOW_MODE_TIMEOUT, MEA_SHUTDOWN_DETECTED, + ARP_INIT_OK, + ARP_INIT_ERROR, + ARP_CAL_DONE, + ARP_FIX_ANTENNAS, + ARP_FIX_ROCKET, DPL_CUT_DROGUE, DPL_CUT_TIMEOUT, DPL_NC_OPEN, @@ -125,6 +130,15 @@ enum Events : uint8_t TMTC_START_RECORDING, TMTC_STOP_RECORDING, TMTC_OPEN_NITROGEN, + TMTC_ARP_FORCE_INIT, + TMTC_ARP_RESET_ALGORITHM, + TMTC_ARP_RESET_BOARD, + TMTC_ARP_FORCE_NO_FEEDBACK, + TMTC_ARP_ARM, + TMTC_ARP_DISARM, + TMTC_ARP_CALIBRATE, + TMTC_ARP_ENTER_TEST_MODE, + TMTC_ARP_EXIT_TEST_MODE, MOTOR_START_TARS, MOTOR_STOP_TARS, MOTOR_OPEN_VENTING_VALVE, @@ -173,6 +187,11 @@ inline std::string getEventString(uint8_t event) {MEA_FORCE_STOP, "MEA_FORCE_STOP"}, {MEA_SHADOW_MODE_TIMEOUT, "MEA_SHADOW_MODE_TIMEOUT"}, {MEA_SHUTDOWN_DETECTED, "MEA_SHUTDOWN_DETECTED"}, + {ARP_INIT_OK, "ARP_INIT_OK"}, + {ARP_INIT_ERROR, "ARP_INIT_ERROR"}, + {ARP_CAL_DONE, "ARP_CAL_DONE"}, + {ARP_FIX_ANTENNAS, "ARP_FIX_ANTENNAS"}, + {ARP_FIX_ROCKET, "ARP_FIX_ROCKET"}, {DPL_CUT_DROGUE, "DPL_CUT_DROGUE"}, {DPL_CUT_TIMEOUT, "DPL_CUT_TIMEOUT"}, {DPL_NC_OPEN, "DPL_NC_OPEN"}, @@ -243,6 +262,15 @@ inline std::string getEventString(uint8_t event) {TMTC_START_RECORDING, "TMTC_START_RECORDING"}, {TMTC_STOP_RECORDING, "TMTC_STOP_RECORDING"}, {TMTC_OPEN_NITROGEN, "TMTC_OPEN_NITROGEN"}, + {TMTC_ARP_FORCE_INIT, "TMTC_ARP_FORCE_INIT"}, + {TMTC_ARP_RESET_ALGORITHM, "TMTC_ARP_RESET_ALGORITHM"}, + {TMTC_ARP_RESET_BOARD, "TMTC_ARP_RESET_BOARD"}, + {TMTC_ARP_FORCE_NO_FEEDBACK, "TMTC_ARP_FORCE_NO_FEEDBACK"}, + {TMTC_ARP_ARM, "TMTC_ARP_ARM"}, + {TMTC_ARP_DISARM, "TMTC_ARP_DISARM"}, + {TMTC_ARP_CALIBRATE, "TMTC_ARP_CALIBRATE"}, + {TMTC_ARP_ENTER_TEST_MODE, "TMTC_ARP_ENTER_TEST_MODE"}, + {TMTC_ARP_EXIT_TEST_MODE, "TMTC_ARP_EXIT_TEST_MODE"}, {MOTOR_START_TARS, "MOTOR_START_TARS"}, {MOTOR_STOP_TARS, "MOTOR_STOP_TARS"}, {MOTOR_OPEN_VENTING_VALVE, "MOTOR_OPEN_VENTING_VALVE"}, diff --git a/src/boards/common/Topics.h b/src/boards/common/Topics.h index 8855f866f5776048489d3e7a36b63107658f5985..8bcf969bff9c7f57b75ef4967942afb0b1773e29 100644 --- a/src/boards/common/Topics.h +++ b/src/boards/common/Topics.h @@ -34,6 +34,7 @@ enum Topics : uint8_t TOPIC_ABK, TOPIC_ADA, TOPIC_MEA, + TOPIC_ARP, TOPIC_DPL, TOPIC_CAN, TOPIC_FLIGHT, @@ -48,9 +49,9 @@ enum Topics : uint8_t }; const std::vector<uint8_t> TOPICS_LIST{ - TOPIC_ABK, TOPIC_ADA, TOPIC_MEA, TOPIC_DPL, TOPIC_CAN, - TOPIC_FLIGHT, TOPIC_FMM, TOPIC_FSR, TOPIC_NAS, TOPIC_TMTC, - TOPIC_MOTOR, TOPIC_TARS, TOPIC_ALT, TOPIC_WING, + TOPIC_ABK, TOPIC_ADA, TOPIC_MEA, TOPIC_ARP, TOPIC_DPL, + TOPIC_CAN, TOPIC_FLIGHT, TOPIC_FMM, TOPIC_FSR, TOPIC_NAS, + TOPIC_TMTC, TOPIC_MOTOR, TOPIC_TARS, TOPIC_ALT, TOPIC_WING }; } // namespace Common diff --git a/src/entrypoints/Groundstation/Automated/test-smcontroller.cpp b/src/entrypoints/Groundstation/Automated/test-smcontroller.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9df24bf57a7e4092df59e108b55cd6a3899461e5 --- /dev/null +++ b/src/entrypoints/Groundstation/Automated/test-smcontroller.cpp @@ -0,0 +1,201 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Federico Lolli + * + * 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 <Groundstation/Automated/SMController/SMController.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <events/EventBroker.h> +#include <utils/Debug.h> + +#include <memory> +#include <unordered_set> + +#define ARP_EVENTS \ + { \ + ARP_INIT_OK, ARP_INIT_ERROR, ARP_CAL_DONE, ARP_FIX_ANTENNAS, \ + ARP_FIX_ROCKET \ + } + +#define TMTC_EVENTS \ + { \ + TMTC_ARP_FORCE_INIT, TMTC_ARP_RESET_ALGORITHM, \ + TMTC_ARP_FORCE_NO_FEEDBACK, TMTC_ARP_ARM, TMTC_ARP_DISARM, \ + TMTC_ARP_CALIBRATE, TMTC_ARP_ENTER_TEST_MODE, \ + TMTC_ARP_EXIT_TEST_MODE \ + } + +#define TEST_STATE(INITIAL_STATE, EVENT, TOPIC_SM, FINAL_STATE) \ + sm->forceState(&SMController::INITIAL_STATE); \ + EventBroker::getInstance().post(EVENT, TOPIC_SM); \ + Thread::sleep(20); \ + TRACE("Testing %-26s in " #INITIAL_STATE " -> " #FINAL_STATE " ... ", \ + getEventString(EVENT).c_str()); \ + correct = sm->testState(&SMController::FINAL_STATE); \ + printf(correct ? "OK\n" : "FAIL\n"); \ + ok &= correct; + +#define TEST_NO_TRANSITION(INITIAL_STATE, EVENT, TOPIC_SM) \ + sm->forceState(&SMController::INITIAL_STATE); \ + EventBroker::getInstance().post(EVENT, TOPIC_SM); \ + Thread::sleep(20); \ + TRACE("Testing %-26s in " #INITIAL_STATE " -X ... ", \ + getEventString(EVENT).c_str()); \ + correct = sm->testState(&SMController::INITIAL_STATE); \ + printf(correct ? "OK\n" : "FAIL\n"); \ + ok &= correct; + +#define TEST_ALL_OTHER(INITIAL_STATE, ...) \ + to_exclude = {__VA_ARGS__}; \ + for (Events ev : arp_events) \ + { \ + if (to_exclude.find(ev) == to_exclude.end()) \ + { \ + TEST_NO_TRANSITION(INITIAL_STATE, ev, TOPIC_ARP); \ + } \ + } \ + for (Events ev : tmtc_events) \ + { \ + if (to_exclude.find(ev) == to_exclude.end()) \ + { \ + TEST_NO_TRANSITION(INITIAL_STATE, ev, TOPIC_TMTC); \ + } \ + } + +// for (auto state : {__VA_ARGS__}) +// { + +using namespace Boardcore; +using namespace Common; +using namespace Antennas; + +SMController* sm = new SMController(); + +int main() +{ + bool correct, ok = true; + std::unordered_set<Events> to_exclude; + std::vector<Events> arp_events = ARP_EVENTS; + std::vector<Events> tmtc_events = TMTC_EVENTS; + + sm->start(); + TRACE("SMController started\n"); + + // TEST STATE: INIT + TEST_STATE(state_init, ARP_INIT_OK, TOPIC_ARP, state_init_done); + TEST_STATE(state_init, ARP_INIT_ERROR, TOPIC_ARP, state_init_error); + TEST_ALL_OTHER(state_init, ARP_INIT_OK, ARP_INIT_ERROR); + + // TEST STATE: INIT_DONE + TEST_STATE(state_init_done, TMTC_ARP_ARM, TOPIC_TMTC, state_armed); + TEST_STATE(state_init_done, TMTC_ARP_FORCE_NO_FEEDBACK, TOPIC_TMTC, + state_insert_info); + TEST_ALL_OTHER(state_init_done, TMTC_ARP_ARM, TMTC_ARP_FORCE_NO_FEEDBACK); + + // TEST STATE: INIT_ERROR + TEST_STATE(state_init_error, TMTC_ARP_FORCE_INIT, TOPIC_TMTC, + state_init_done); + TEST_STATE(state_init_error, TMTC_ARP_FORCE_NO_FEEDBACK, TOPIC_TMTC, + state_insert_info); + TEST_ALL_OTHER(state_init_error, TMTC_ARP_FORCE_INIT, + TMTC_ARP_FORCE_NO_FEEDBACK); + + // TEST STATE: INSERT_INFO + TEST_STATE(state_insert_info, TMTC_ARP_ARM, TOPIC_TMTC, state_armed_nf); + TEST_ALL_OTHER(state_insert_info, TMTC_ARP_ARM); + + // TEST STATE: ARMED + TEST_STATE(state_armed, TMTC_ARP_DISARM, TOPIC_TMTC, state_init_done); + TEST_STATE(state_armed, TMTC_ARP_CALIBRATE, TOPIC_TMTC, state_calibrate); + TEST_STATE(state_armed, TMTC_ARP_ENTER_TEST_MODE, TOPIC_TMTC, state_test); + TEST_ALL_OTHER(state_armed, TMTC_ARP_DISARM, TMTC_ARP_CALIBRATE, + TMTC_ARP_ENTER_TEST_MODE); + + // TEST STATE: TEST + TEST_STATE(state_test, TMTC_ARP_EXIT_TEST_MODE, TOPIC_TMTC, state_armed); + TEST_STATE(state_test, TMTC_ARP_DISARM, TOPIC_TMTC, state_init_done); + TEST_ALL_OTHER(state_test, TMTC_ARP_EXIT_TEST_MODE, TMTC_ARP_DISARM); + + // TEST STATE: CALIBRATE + TEST_STATE(state_calibrate, ARP_CAL_DONE, TOPIC_ARP, state_fix_antennas); + TEST_STATE(state_calibrate, TMTC_ARP_DISARM, TOPIC_TMTC, state_init_done); + TEST_STATE(state_calibrate, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC, + state_armed); + TEST_ALL_OTHER(state_calibrate, ARP_CAL_DONE, TMTC_ARP_DISARM, + TMTC_ARP_RESET_ALGORITHM); + + // TEST STATE: FIX_ANTENNAS + TEST_STATE(state_fix_antennas, ARP_FIX_ANTENNAS, TOPIC_ARP, + state_fix_rocket); + TEST_STATE(state_fix_antennas, TMTC_ARP_DISARM, TOPIC_TMTC, + state_init_done); + TEST_STATE(state_fix_antennas, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC, + state_armed); + TEST_ALL_OTHER(state_fix_antennas, ARP_FIX_ANTENNAS, TMTC_ARP_DISARM, + TMTC_ARP_RESET_ALGORITHM); + + // TEST STATE: FIX_ROCKET + TEST_STATE(state_fix_rocket, ARP_FIX_ROCKET, TOPIC_ARP, state_active); + TEST_STATE(state_fix_rocket, TMTC_ARP_DISARM, TOPIC_TMTC, state_init_done); + TEST_STATE(state_fix_rocket, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC, + state_armed); + TEST_ALL_OTHER(state_fix_rocket, ARP_FIX_ROCKET, TMTC_ARP_DISARM, + TMTC_ARP_RESET_ALGORITHM); + + // TEST STATE: ACTIVE + TEST_STATE(state_active, TMTC_ARP_DISARM, TOPIC_TMTC, state_init_done); + TEST_STATE(state_active, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC, state_armed); + TEST_ALL_OTHER(state_active, TMTC_ARP_DISARM, TMTC_ARP_RESET_ALGORITHM); + + // TEST STATE: ARMED_NO_FEEDBACK + TEST_STATE(state_armed_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_insert_info); + TEST_STATE(state_armed_nf, TMTC_ARP_CALIBRATE, TOPIC_TMTC, + state_fix_rocket_nf); + TEST_STATE(state_armed_nf, TMTC_ARP_ENTER_TEST_MODE, TOPIC_TMTC, + state_test_nf); + TEST_ALL_OTHER(state_armed_nf, TMTC_ARP_DISARM, TMTC_ARP_CALIBRATE, + TMTC_ARP_ENTER_TEST_MODE); + + // TEST STATE: TEST_NO_FEEDBACK + TEST_STATE(state_test_nf, TMTC_ARP_EXIT_TEST_MODE, TOPIC_TMTC, + state_armed_nf); + TEST_STATE(state_test_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_insert_info); + TEST_ALL_OTHER(state_test_nf, TMTC_ARP_EXIT_TEST_MODE, TMTC_ARP_DISARM); + + // TEST STATE: FIX_ROCKET_NO_FEEDBACK + TEST_STATE(state_fix_rocket_nf, ARP_FIX_ROCKET, TOPIC_ARP, state_active_nf); + TEST_STATE(state_fix_rocket_nf, TMTC_ARP_DISARM, TOPIC_TMTC, + state_insert_info); + TEST_STATE(state_fix_rocket_nf, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC, + state_armed_nf); + TEST_ALL_OTHER(state_fix_rocket_nf, ARP_FIX_ROCKET, TMTC_ARP_DISARM, + TMTC_ARP_RESET_ALGORITHM); + + // TEST STATE: ACTIVE_NO_FEEDBACK + TEST_STATE(state_active_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_insert_info); + TEST_STATE(state_active_nf, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC, + state_armed_nf); + TEST_ALL_OTHER(state_active_nf, TMTC_ARP_DISARM, TMTC_ARP_RESET_ALGORITHM); + + TRACE("Testing SMController ... "); + ok ? printf("OK\n") : printf("FAIL\n"); + return 0; +}