diff --git a/CMakeLists.txt b/CMakeLists.txt
index e653dae887135ee1206bc11368ba96406226e108..2fa2ec6a0029a231cf8e429bc4993fef579c0889 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -72,7 +72,7 @@ sbs_target(rig-v2-entry stm32f767zi_rig_v2)
 
 add_executable(con_rig-entry src/entrypoints/con_RIG/con_rig-entry.cpp ${CON_RIG_COMPUTER})
 target_include_directories(con_rig-entry PRIVATE ${OBSW_INCLUDE_DIRS})
-sbs_target(con_rig-entry stm32f429zi_stm32f4discovery)
+sbs_target(con_rig-entry stm32f429zi_con_rig)
 
 add_executable(base-groundstation-entry 
     src/entrypoints/Groundstation/base-groundstation-entry.cpp 
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index cedb525955a483016d0afab5bf5264fe39cd8fc3..61e69d10c9ad21296f25db2132323f5e2d12076a 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -73,7 +73,6 @@ set(RIG_V2_COMPUTER
 set(CON_RIG_COMPUTER
     src/boards/con_RIG/Buttons/Buttons.cpp
     src/boards/con_RIG/Radio/Radio.cpp
-    src/boards/con_RIG/BoardScheduler.cpp
 )
 
 set(PAYLOAD_COMPUTER
diff --git a/src/boards/con_RIG/BoardScheduler.cpp b/src/boards/con_RIG/BoardScheduler.cpp
deleted file mode 100644
index a9371f3e4239f586392b3cbc45219f8cbe623320..0000000000000000000000000000000000000000
--- a/src/boards/con_RIG/BoardScheduler.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Authors: Matteo Pignataro
- *
- * 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 "BoardScheduler.h"
-
-using namespace Boardcore;
-
-namespace con_RIG
-{
-// TODO: UPDATE THE SCHEDULER PRIORITY PARAMETER ONCE MERGED NEW TASK SCHEDULER
-BoardScheduler::BoardScheduler()
-{
-    scheduler1 = new TaskScheduler(miosix::PRIORITY_MAX - 4);
-    scheduler2 = new TaskScheduler(miosix::PRIORITY_MAX - 3);
-    scheduler3 = new TaskScheduler(miosix::PRIORITY_MAX - 2);
-    scheduler4 = new TaskScheduler(miosix::PRIORITY_MAX - 1);
-}
-
-TaskScheduler* BoardScheduler::getScheduler(miosix::Priority priority)
-{
-    switch (priority.get())
-    {
-        case miosix::PRIORITY_MAX:
-            return scheduler4;
-        case miosix::PRIORITY_MAX - 1:
-            return scheduler3;
-        case miosix::PRIORITY_MAX - 2:
-            return scheduler2;
-        case miosix::MAIN_PRIORITY:
-            return scheduler1;
-        default:
-            return scheduler1;
-    }
-}
-
-bool BoardScheduler::start()
-{
-    return scheduler1->start() && scheduler2->start() && scheduler3->start() &&
-           scheduler4->start();
-}
-
-bool BoardScheduler::isStarted()
-{
-    return scheduler1->isRunning() && scheduler2->isRunning() &&
-           scheduler3->isRunning() && scheduler4->isRunning();
-}
-}  // namespace con_RIG
\ No newline at end of file
diff --git a/src/boards/con_RIG/BoardScheduler.h b/src/boards/con_RIG/BoardScheduler.h
index 9fe83776e8aee7421c1b264b1299833fcc6e85fe..df9359092b920f7c42aabed44991d9eb9d6256a0 100644
--- a/src/boards/con_RIG/BoardScheduler.h
+++ b/src/boards/con_RIG/BoardScheduler.h
@@ -36,29 +36,19 @@ namespace con_RIG
 class BoardScheduler : public Boardcore::Module
 {
 public:
-    BoardScheduler();
+    BoardScheduler()
+        : radio{miosix::PRIORITY_MAX - 1}, buttons{miosix::PRIORITY_MAX - 2}
+    {
+    }
 
-    /**
-     * @brief Get the Scheduler object relative to the requested priority
-     *
-     * @param priority The task scheduler priority
-     * @return Boardcore::TaskScheduler& Reference to the requested task
-     * scheduler.
-     * @note Min priority scheduler is returned in case of non valid priority.
-     */
-    Boardcore::TaskScheduler* getScheduler(miosix::Priority priority);
+    [[nodiscard]] bool start() { return radio.start() && buttons.start(); }
 
-    [[nodiscard]] bool start();
+    Boardcore::TaskScheduler &getRadioScheduler() { return radio; }
 
-    /**
-     * @brief Returns if all the schedulers are up and running
-     */
-    bool isStarted();
+    Boardcore::TaskScheduler &getButtonsScheduler() { return buttons; }
 
 private:
-    Boardcore::TaskScheduler* scheduler1;
-    Boardcore::TaskScheduler* scheduler2;
-    Boardcore::TaskScheduler* scheduler3;
-    Boardcore::TaskScheduler* scheduler4;
+    Boardcore::TaskScheduler radio;
+    Boardcore::TaskScheduler buttons;
 };
 }  // namespace con_RIG
diff --git a/src/boards/con_RIG/Buses.h b/src/boards/con_RIG/Buses.h
index 7fb368dade3952e30df1c26807f0d43db6e648ca..1afa71acb86387677c8fd4ddc33d1323c497ebf2 100644
--- a/src/boards/con_RIG/Buses.h
+++ b/src/boards/con_RIG/Buses.h
@@ -33,11 +33,13 @@ namespace con_RIG
 
 struct Buses : public Boardcore::Module
 {
+private:
     Boardcore::SPIBus spi1;
-    Boardcore::SPIBus spi2;
 
 public:
-    Buses() : spi1(SPI1), spi2(SPI2) {}
+    Buses() : spi1(SPI1) {}
+
+    Boardcore::SPIBus &getRadio() { return spi1; }
 };
 
 }  // namespace con_RIG
diff --git a/src/boards/con_RIG/Buttons/Buttons.cpp b/src/boards/con_RIG/Buttons/Buttons.cpp
index 5a604346d62281d9ae92554c123d1359200584ea..5e32ad2e081d72729a097c5bbd0ad9e2b58f4718 100644
--- a/src/boards/con_RIG/Buttons/Buttons.cpp
+++ b/src/boards/con_RIG/Buttons/Buttons.cpp
@@ -25,31 +25,23 @@
 #include <con_RIG/BoardScheduler.h>
 #include <con_RIG/Configs/ButtonsConfig.h>
 #include <con_RIG/Radio/Radio.h>
+#include <interfaces-impl/hwmapping.h>
 
 using namespace std;
 using namespace miosix;
 using namespace Boardcore;
-using namespace con_RIG::Config::Buttons;
+using namespace con_RIG;
 
-namespace con_RIG
-{
-
-Buttons::Buttons(TaskScheduler* sched) : scheduler(sched)
+Buttons::Buttons(TaskScheduler& scheduler) : scheduler(scheduler)
 {
     resetState();
     state.arm_switch = false;
-    remoteArm        = 0;
 }
 
 bool Buttons::start()
 {
-    return scheduler->addTask([&]() { periodicStatusCheck(); },
-                              BUTTON_SAMPLE_PERIOD);
-}
-
-Buttons::~Buttons()
-{
-    // Delete all the buttons
+    return scheduler.addTask([this]() { periodicStatusCheck(); },
+                             Config::Buttons::BUTTON_SAMPLE_PERIOD) != 0;
 }
 
 mavlink_conrig_state_tc_t Buttons::getState() { return state; }
@@ -66,18 +58,9 @@ void Buttons::resetState()
 
 void Buttons::periodicStatusCheck()
 {
-    // TODO: This should be in bsp
-    using GpioIgnitionBtn        = Gpio<GPIOB_BASE, 4>;
-    using GpioFillingValveBtn    = Gpio<GPIOE_BASE, 6>;
-    using GpioVentingValveBtn    = Gpio<GPIOE_BASE, 4>;
-    using GpioReleasePressureBtn = Gpio<GPIOG_BASE, 9>;
-    using GpioQuickConnectorBtn  = Gpio<GPIOD_BASE, 7>;
-    using GpioStartTarsBtn       = Gpio<GPIOD_BASE, 5>;
-    using GpioArmedSwitch        = Gpio<GPIOE_BASE, 2>;
-
-    state.arm_switch = GpioArmedSwitch::getPin().value();
+    state.arm_switch = btns::arm::value();
 
-    if (!GpioIgnitionBtn::getPin().value() && state.arm_switch)
+    if (!btns::ignition::value() && state.arm_switch)
     {
         if (guard > Config::Buttons::GUARD_THRESHOLD)
         {
@@ -89,7 +72,7 @@ void Buttons::periodicStatusCheck()
             guard++;
         }
     }
-    else if (GpioFillingValveBtn::getPin().value())
+    else if (btns::filling::value())
     {
         if (guard > Config::Buttons::GUARD_THRESHOLD)
         {
@@ -101,7 +84,7 @@ void Buttons::periodicStatusCheck()
             guard++;
         }
     }
-    else if (GpioVentingValveBtn::getPin().value())
+    else if (btns::venting::value())
     {
         if (guard > Config::Buttons::GUARD_THRESHOLD)
         {
@@ -113,7 +96,7 @@ void Buttons::periodicStatusCheck()
             guard++;
         }
     }
-    else if (GpioReleasePressureBtn::getPin().value())
+    else if (btns::release::value())
     {
         if (guard > Config::Buttons::GUARD_THRESHOLD)
         {
@@ -125,7 +108,7 @@ void Buttons::periodicStatusCheck()
             guard++;
         }
     }
-    else if (GpioQuickConnectorBtn::getPin().value())
+    else if (btns::detach::value())
     {
         if (guard > Config::Buttons::GUARD_THRESHOLD)
         {
@@ -137,7 +120,7 @@ void Buttons::periodicStatusCheck()
             guard++;
         }
     }
-    else if (GpioStartTarsBtn::getPin().value())
+    else if (btns::tars::value())
     {
         if (guard > Config::Buttons::GUARD_THRESHOLD)
         {
@@ -164,21 +147,6 @@ void Buttons::periodicStatusCheck()
     //        state.detach_quick_connector, state.startup_tars, state.armed);
 }
 
-// 1 if rocket is armed, 0 if disarmed
-void Buttons::setRemoteArmState(int armed)
-{
-    using armed_led = Gpio<GPIOC_BASE, 13>;
-
-    if (armed)
-    {
-        remoteArm = true;
-        armed_led::high();
-    }
-    else
-    {
-        remoteArm = false;
-        armed_led::low();
-    }
-}
+void Buttons::enableIgnition() { ui::armedLed::high(); }
 
-}  // namespace con_RIG
+void Buttons::disableIgnition() { ui::armedLed::low(); }
diff --git a/src/boards/con_RIG/Buttons/Buttons.h b/src/boards/con_RIG/Buttons/Buttons.h
index e7c5f078918b6d4f570dc5b7072c1a4ad3d8baa1..43dea1eae341a57644daa5076544d3faa4540368 100644
--- a/src/boards/con_RIG/Buttons/Buttons.h
+++ b/src/boards/con_RIG/Buttons/Buttons.h
@@ -35,27 +35,26 @@ class Buttons : public Boardcore::Module
 {
 
 public:
-    explicit Buttons(Boardcore::TaskScheduler* sched);
+    explicit Buttons(Boardcore::TaskScheduler& scheduler);
 
-    ~Buttons();
-
-    bool start();
+    [[nodiscard]] bool start();
 
     mavlink_conrig_state_tc_t getState();
 
+    void enableIgnition();
+    void disableIgnition();
+
+private:
     void resetState();
 
-    void setRemoteArmState(int state);
+    void periodicStatusCheck();
 
-private:
     mavlink_conrig_state_tc_t state;
-    void periodicStatusCheck();
-    std::atomic<bool> remoteArm{false};
 
     // Counter guard to avoid spurious triggers
     uint8_t guard = 0;
 
-    Boardcore::TaskScheduler* scheduler = nullptr;
+    Boardcore::TaskScheduler& scheduler;
 
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("buttons");
 };
diff --git a/src/boards/con_RIG/Configs/ButtonsConfig.h b/src/boards/con_RIG/Configs/ButtonsConfig.h
index 671d55fd46c5834d0b651aebcd60b93a1cf4d9a4..5833c15504610efbed6f8cacdb597f6277ab1e98 100644
--- a/src/boards/con_RIG/Configs/ButtonsConfig.h
+++ b/src/boards/con_RIG/Configs/ButtonsConfig.h
@@ -38,14 +38,7 @@ namespace Buttons
 
 static constexpr uint32_t BUTTON_SAMPLE_PERIOD = 20;  // 50Hz
 
-constexpr uint8_t CHECK_BUTTON_STATE_TASK_ID = 150;
-
-static constexpr uint32_t BUZZER_PERIOD = 100;
-static constexpr uint32_t BUZZER_DELAY  = 3000;
-constexpr uint8_t BUZZER_ON_TASK_ID     = 160;
-constexpr uint8_t BUZZER_OFF_TASK_ID    = 161;
-
-constexpr uint8_t GUARD_THRESHOLD =
+static constexpr uint8_t GUARD_THRESHOLD =
     5;  // 5 samples to trigger the guard and activate a single button
 
 }  // namespace Buttons
diff --git a/src/boards/con_RIG/Configs/RadioConfig.h b/src/boards/con_RIG/Configs/RadioConfig.h
index 3ad8d010ebde8bd0eb959cda27a0bbaebd57f179..8e820b035a6f14fc2e20fe4096971071c7592aab 100644
--- a/src/boards/con_RIG/Configs/RadioConfig.h
+++ b/src/boards/con_RIG/Configs/RadioConfig.h
@@ -32,25 +32,20 @@ namespace Config
 namespace Radio
 {
 
-// Mavlink driver template parameters
-constexpr uint32_t RADIO_PKT_LENGTH     = 255;
-constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 10;
-constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
+static constexpr unsigned int MAV_OUT_QUEUE_SIZE = 20;
+static constexpr unsigned int MAV_MAX_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE;
 
-// Mavlink driver parameters
-constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 200;
+static constexpr uint16_t MAV_SLEEP_AFTER_SEND = 0;
+static constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 10;
 
 // Mavlink ids
-constexpr uint8_t MAV_SYSTEM_ID    = 171;
-constexpr uint8_t MAV_COMPONENT_ID = 96;
+static constexpr uint8_t MAV_SYSTEM_ID    = 171;
+static constexpr uint8_t MAV_COMPONENT_ID = 96;
 
 // Periodic telemetries frequency
 constexpr uint32_t PING_GSE_PERIOD = 500;  // [ms]
 
-// Periodic telemetries tasks ids
-constexpr uint8_t PING_GSE_TASK_ID = 200;
-
-constexpr uint32_t MAVLINK_QUEUE_SIZE = 3;
+static constexpr uint32_t MAVLINK_QUEUE_SIZE = 3;
 
 }  // namespace Radio
 }  // namespace Config
diff --git a/src/boards/con_RIG/Radio/Radio.cpp b/src/boards/con_RIG/Radio/Radio.cpp
index 5040b62ea3b96d0512fb5844c106b0916f0ce426..b9e3b5513783d3df02ffbc6fa2db02b98a254b61 100644
--- a/src/boards/con_RIG/Radio/Radio.cpp
+++ b/src/boards/con_RIG/Radio/Radio.cpp
@@ -22,69 +22,67 @@
 
 #include "Radio.h"
 
-#include <common/MavlinkGemini.h>
+#include <common/Mavlink.h>
+#include <common/Radio.h>
 #include <con_RIG/BoardScheduler.h>
 #include <con_RIG/Buses.h>
 #include <con_RIG/Buttons/Buttons.h>
 #include <diagnostic/SkywardStack.h>
 #include <drivers/interrupt/external_interrupts.h>
 #include <events/EventBroker.h>
+#include <interfaces-impl/hwmapping.h>
 #include <radio/SX1278/SX1278Frontends.h>
-#include <radio/Xbee/ATCommands.h>
 
 #include <thread>
 
-using namespace std;
 using namespace miosix;
-using namespace placeholders;
 using namespace Boardcore;
-using namespace con_RIG::Config::Radio;
+using namespace con_RIG;
+using namespace Common;
 
-void __attribute__((used)) EXTI1_IRQHandlerImpl()
-{
-    ModuleManager& modules = ModuleManager::getInstance();
-    if (modules.get<con_RIG::Radio>()->transceiver != nullptr)
-    {
-        modules.get<con_RIG::Radio>()->transceiver->handleDioIRQ();
-    }
-}
+SX1278Lora* gRadio{nullptr};
 
-void __attribute__((used)) EXTI12_IRQHandlerImpl()
+void handleDioIRQ()
 {
-    ModuleManager& modules = ModuleManager::getInstance();
-    if (modules.get<con_RIG::Radio>()->transceiver != nullptr)
+    SX1278Lora* instance = gRadio;
+    if (instance)
     {
-        modules.get<con_RIG::Radio>()->transceiver->handleDioIRQ();
+        instance->handleDioIRQ();
     }
 }
 
-void __attribute__((used)) EXTI13_IRQHandlerImpl()
+void setIRQRadio(SX1278Lora* radio)
 {
-    ModuleManager& modules = ModuleManager::getInstance();
-    if (modules.get<con_RIG::Radio>()->transceiver != nullptr)
-    {
-        modules.get<con_RIG::Radio>()->transceiver->handleDioIRQ();
-    }
+    FastInterruptDisableLock dl;
+    gRadio = radio;
 }
 
-namespace con_RIG
-{
+void __attribute__((used)) MIOSIX_RADIO_DIO0_IRQ() { handleDioIRQ(); }
+void __attribute__((used)) MIOSIX_RADIO_DIO1_IRQ() { handleDioIRQ(); }
+void __attribute__((used)) MIOSIX_RADIO_DIO3_IRQ() { handleDioIRQ(); }
 
-void Radio::handleMavlinkMessage(MavDriver* driver,
-                                 const mavlink_message_t& msg)
+void Radio::handleMessage(const mavlink_message_t& msg)
 {
+    ModuleManager& modules = ModuleManager::getInstance();
     switch (msg.msgid)
     {
         case MAVLINK_MSG_ID_GSE_TM:
         {
-            int arming_state = mavlink_msg_gse_tm_get_arming_state(&msg);
-            ModuleManager::getInstance().get<Buttons>()->setRemoteArmState(
-                arming_state);
-            messageReceived +=
-                arming_state == 1
-                    ? 10
-                    : 2;  // The beep increments in speed as the state is armed
+            int armingState = mavlink_msg_gse_tm_get_arming_state(&msg);
+            if (armingState == 1)
+            {
+                modules.get<Buttons>()->enableIgnition();
+                messageReceived += 10;
+            }
+            else
+            {
+                modules.get<Buttons>()->disableIgnition();
+                messageReceived += 2;
+            }
+
+            break;
         }
+
         case MAVLINK_MSG_ID_ACK_TM:
         {
             int id = mavlink_msg_ack_tm_get_recv_msgid(&msg);
@@ -101,8 +99,11 @@ void Radio::handleMavlinkMessage(MavDriver* driver,
                 buttonState.start_tars_btn       = false;
                 buttonState.arm_switch           = false;
             }
+
+            break;
         }
     }
+
     mavlinkWriteToUsart(msg);
 }
 
@@ -144,7 +145,6 @@ void Radio::loopReadFromUsart()
 {
     mavlink_status_t status;
     mavlink_message_t msg;
-    int chan = 0;  // TODO: what is this?
     uint8_t byte;
 
     while (true)
@@ -152,9 +152,9 @@ void Radio::loopReadFromUsart()
         auto serial = miosix::DefaultConsole::instance().get();
         int len     = serial->readBlock(&byte, 1, 0);
 
-        if (len && mavlink_parse_char(chan, byte, &msg, &status))
+        if (len && mavlink_parse_char(MAVLINK_COMM_0, byte, &msg, &status))
         {
-            if (message_queue_index == MAVLINK_QUEUE_SIZE - 1)
+            if (message_queue_index == Config::Radio::MAVLINK_QUEUE_SIZE - 1)
             {
                 mavlink_message_t nack_msg;
                 mavlink_nack_tm_t nack_tm;
@@ -180,68 +180,63 @@ void Radio::setInternalState(mavlink_conrig_state_tc_t state)
     Lock<FastMutex> lock(internalStateMutex);
     // The OR operator is introduced to make sure that the receiver
     // understood the command
-    buttonState.ignition_btn = state.ignition_btn || buttonState.ignition_btn;
-    buttonState.filling_valve_btn =
-        state.filling_valve_btn || buttonState.filling_valve_btn;
-    buttonState.venting_valve_btn =
-        state.venting_valve_btn || buttonState.venting_valve_btn;
-    buttonState.release_pressure_btn =
-        state.release_pressure_btn || buttonState.release_pressure_btn;
-    buttonState.quick_connector_btn =
-        state.quick_connector_btn || buttonState.quick_connector_btn;
-    buttonState.start_tars_btn =
-        state.start_tars_btn || buttonState.start_tars_btn;
-    buttonState.arm_switch = state.arm_switch || buttonState.arm_switch;
+    buttonState.ignition_btn |= state.ignition_btn;
+    buttonState.filling_valve_btn |= state.filling_valve_btn;
+    buttonState.venting_valve_btn |= state.venting_valve_btn;
+    buttonState.release_pressure_btn |= state.release_pressure_btn;
+    buttonState.quick_connector_btn |= state.quick_connector_btn;
+    buttonState.start_tars_btn |= state.start_tars_btn;
+    buttonState.arm_switch |= state.arm_switch;
 }
 
 bool Radio::start()
 {
-    // TODO: constants should be in bps
-    using dio0 = Gpio<GPIOB_BASE, 1>;
-    using dio1 = Gpio<GPIOD_BASE, 12>;
-    using dio3 = Gpio<GPIOD_BASE, 13>;
-    using txEn = Gpio<GPIOG_BASE, 2>;
-    using rxEn = Gpio<GPIOG_BASE, 3>;
-    using cs   = Gpio<GPIOF_BASE, 6>;
-
     ModuleManager& modules = ModuleManager::getInstance();
 
-    // Config the transceiver
-    SX1278Lora::Config config{};
-    config.power            = 2;
-    config.ocp              = 0;  // Over current protection
-    config.coding_rate      = SX1278Lora::Config::Cr::CR_1;
-    config.spreading_factor = SX1278Lora::Config::Sf::SF_7;
-
+    // Setup the frontend
     std::unique_ptr<SX1278::ISX1278Frontend> frontend =
-        std::make_unique<EbyteFrontend>(txEn::getPin(), rxEn::getPin());
+        std::make_unique<EbyteFrontend>(radio::txEn::getPin(),
+                                        radio::rxEn::getPin());
 
-    transceiver =
-        new SX1278Lora(modules.get<Buses>()->spi1, cs::getPin(), dio0::getPin(),
-                       dio1::getPin(), dio3::getPin(),
-                       SPI::ClockDivider::DIV_64, std::move(frontend));
+    // Setup transceiver
+    radio = std::make_unique<SX1278Lora>(
+        modules.get<Buses>()->getRadio(), radio::cs::getPin(),
+        radio::dio0::getPin(), radio::dio1::getPin(), radio::dio3::getPin(),
+        SPI::ClockDivider::DIV_64, std::move(frontend));
 
-    SX1278Lora::Error error = transceiver->init(config);
+    // Store the global radio instance
+    setIRQRadio(radio.get());
 
-    // Config mavDriver
-    mavDriver = new MavDriver(transceiver,
-                              bind(&Radio::handleMavlinkMessage, this, _1, _2),
-                              0, MAV_OUT_BUFFER_MAX_AGE);
+    // Initialize radio
+    auto result = radio->init(RIG_RADIO_CONFIG);
+    if (result != SX1278Lora::Error::NONE)
+    {
+        LOG_ERR(logger, "Failed to initialize RIG radio");
+        return false;
+    }
+
+    // Initialize mavdriver
+    mavDriver = std::make_unique<MavDriver>(
+        radio.get(),
+        [this](MavDriver*, const mavlink_message_t& msg)
+        { handleMessage(msg); },
+        Config::Radio::MAV_SLEEP_AFTER_SEND,
+        Config::Radio::MAV_OUT_BUFFER_MAX_AGE);
 
-    // In case of failure the mav driver must be created at least
-    if (error != SX1278Lora::Error::NONE)
+    if (!mavDriver->start())
     {
+        LOG_ERR(logger, "Failed to initialize RIG mav driver");
         return false;
     }
 
-    scheduler->addTask([&]() { sendMessages(); }, PING_GSE_PERIOD,
-                       TaskScheduler::Policy::RECOVER);
+    scheduler.addTask([this]() { sendMessages(); },
+                      Config::Radio::PING_GSE_PERIOD,
+                      TaskScheduler::Policy::RECOVER);
 
-    receiverLooper = std::thread([=]() { loopReadFromUsart(); });
+    receiverLooper = std::thread([this]() { loopReadFromUsart(); });
     beeperLooper   = std::thread(
         [&]()
         {
-            using buzzer = Gpio<GPIOB_BASE, 7>;
             while (true)
             {
                 Thread::sleep(100);
@@ -250,9 +245,9 @@ bool Radio::start()
                 if (messageReceived > 5)
                 {
                     messageReceived = 0;
-                    buzzer::low();
+                    ui::buzzer::low();
                     Thread::sleep(100);
-                    buzzer::high();
+                    ui::buzzer::high();
                 }
             }
         });
@@ -260,14 +255,10 @@ bool Radio::start()
     return mavDriver->start();
 }
 
-bool Radio::isStarted() { return mavDriver->isStarted(); }
-
 MavlinkStatus Radio::getMavlinkStatus() { return mavDriver->getStatus(); }
 
-Radio::Radio(TaskScheduler* sched) : scheduler(sched)
+Radio::Radio(TaskScheduler& scheduler) : scheduler{scheduler}
 {
-    Lock<FastMutex> lock(internalStateMutex);
-
     buttonState.ignition_btn         = false;
     buttonState.filling_valve_btn    = false;
     buttonState.venting_valve_btn    = false;
@@ -275,6 +266,4 @@ Radio::Radio(TaskScheduler* sched) : scheduler(sched)
     buttonState.quick_connector_btn  = false;
     buttonState.start_tars_btn       = false;
     buttonState.arm_switch           = false;
-}
-
-}  // namespace con_RIG
+}
\ No newline at end of file
diff --git a/src/boards/con_RIG/Radio/Radio.h b/src/boards/con_RIG/Radio/Radio.h
index 87001c54dc931d3527ba3712cafa99192eb89b98..fc6bfac351b2ae09f38b22a6d452ff5878fa15aa 100644
--- a/src/boards/con_RIG/Radio/Radio.h
+++ b/src/boards/con_RIG/Radio/Radio.h
@@ -36,35 +36,31 @@
 namespace con_RIG
 {
 
-using MavDriver = Boardcore::MavlinkDriver<Config::Radio::RADIO_PKT_LENGTH,
-                                           Config::Radio::RADIO_OUT_QUEUE_SIZE,
-                                           Config::Radio::RADIO_MAV_MSG_LENGTH>;
+using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Lora::MTU,
+                                           Config::Radio::MAV_OUT_QUEUE_SIZE,
+                                           Config::Radio::MAV_MAX_LENGTH>;
 
 class Radio : public Boardcore::Module
 {
 public:
-    explicit Radio(Boardcore::TaskScheduler* sched);
+    explicit Radio(Boardcore::TaskScheduler& scheduler);
 
-    bool start();
-
-    bool isStarted();
+    [[nodiscard]] bool start();
 
     Boardcore::MavlinkStatus getMavlinkStatus();
 
-    void sendMessages();
-
-    void loopReadFromUsart();
-
     void setInternalState(mavlink_conrig_state_tc_t state);
 
-    Boardcore::SX1278Lora* transceiver = nullptr;
-    MavDriver* mavDriver               = nullptr;
-
 private:
-    void handleMavlinkMessage(MavDriver* driver, const mavlink_message_t& msg);
+    void sendMessages();
+    void loopReadFromUsart();
+    void handleMessage(const mavlink_message_t& msg);
 
     void mavlinkWriteToUsart(const mavlink_message_t& msg);
 
+    std::unique_ptr<Boardcore::SX1278Lora> radio;
+    std::unique_ptr<MavDriver> mavDriver;
+
     mavlink_message_t message_queue[Config::Radio::MAVLINK_QUEUE_SIZE];
     uint8_t message_queue_index = 0;
     miosix::FastMutex mutex;
@@ -76,7 +72,7 @@ private:
     std::thread receiverLooper;
     std::thread beeperLooper;
     std::atomic<uint8_t> messageReceived{0};
-    Boardcore::TaskScheduler* scheduler = nullptr;
+    Boardcore::TaskScheduler& scheduler;
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("radio");
 };
 
diff --git a/src/entrypoints/con_RIG/con_rig-entry.cpp b/src/entrypoints/con_RIG/con_rig-entry.cpp
index 5e305fb8b3c396681e7f254e750a3a6368eb3df9..d6371f450053384da7005b82ceeed90b217c37e9 100644
--- a/src/entrypoints/con_RIG/con_rig-entry.cpp
+++ b/src/entrypoints/con_RIG/con_rig-entry.cpp
@@ -28,6 +28,7 @@
 #include <diagnostic/CpuMeter/CpuMeter.h>
 #include <diagnostic/PrintLogger.h>
 #include <events/EventBroker.h>
+#include <interfaces-impl/hwmapping.h>
 #include <miosix.h>
 
 #include <thread>
@@ -115,69 +116,45 @@ void initPins()
 
 int main()
 {
-
-    bool initResult        = true;
-    ModuleManager& modules = ModuleManager::getInstance();
     PrintLogger logger     = Logging::getLogger("main");
-
-    initPins();
+    ModuleManager& modules = ModuleManager::getInstance();
 
     BoardScheduler* scheduler = new BoardScheduler();
     Buses* buses              = new Buses();
-    Radio* radio     = new Radio(scheduler->getScheduler(PRIORITY_MAX));
-    Buttons* buttons = new Buttons(scheduler->getScheduler(PRIORITY_MAX - 1));
-
-    if (!modules.insert<BoardScheduler>(scheduler))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error initializing BoardScheduler module");
-    }
+    Radio* radio              = new Radio(scheduler->getRadioScheduler());
+    Buttons* buttons          = new Buttons(scheduler->getButtonsScheduler());
 
-    if (!modules.insert<Buses>(buses))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error initializing Buses module");
-    }
-
-    if (!modules.insert<Radio>(radio))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error initializing Radio module");
-    }
-
-    if (!modules.insert<Buttons>(buttons))
-    {
-        initResult = false;
-        LOG_ERR(logger, "Error initializing Buttons module");
-    }
+    bool initResult = modules.insert<BoardScheduler>(scheduler) &&
+                      modules.insert<Buses>(buses) &&
+                      modules.insert<Radio>(radio) &&
+                      modules.insert<Buttons>(buttons);
 
-    if (!modules.get<Radio>()->start())
+    if (!radio->start())
     {
         initResult = false;
         LOG_ERR(logger, "Error starting the radio");
     }
 
-    if (!modules.get<Buttons>()->start())
+    if (!buttons->start())
     {
         initResult = false;
         LOG_ERR(logger, "Error starting the buttons");
     }
 
-    if (!modules.get<BoardScheduler>()->start())
+    if (!scheduler->start())
     {
         initResult = false;
         LOG_ERR(logger, "Error starting the General Purpose Scheduler");
     }
 
-    if (initResult)
+    if (!initResult)
     {
-        // POST OK
-        // EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
+        ui::redLed::high();
+        LOG_ERR(logger, "Init failure!");
     }
     else
     {
-        using redLed = Gpio<GPIOG_BASE, 14>;  // Red LED
-        redLed::high();
+        LOG_INFO(logger, "All good!");
     }
 
     // Periodical statistics