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;
+}