diff --git a/.gitmodules b/.gitmodules
index 30a229992e84a25a9323f9a9a446d55157b9b86f..56e8edcdff0f9f7cf3ad7f0b512c111549d1fb93 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,6 @@
 [submodule "skyward-boardcore"]
 	path = skyward-boardcore
 	url = ../skyward-boardcore.git
+[submodule "libs/mavlink_skyward_lib"]
+	path = libs/mavlink_skyward_lib
+	url = https://git.skywarder.eu/r2a/mavlink_skyward_lib.git
diff --git a/libs/mavlink_skyward_lib b/libs/mavlink_skyward_lib
new file mode 160000
index 0000000000000000000000000000000000000000..58c58bb6466f486151bb620bfa7c7fb5bc7a3059
--- /dev/null
+++ b/libs/mavlink_skyward_lib
@@ -0,0 +1 @@
+Subproject commit 58c58bb6466f486151bb620bfa7c7fb5bc7a3059
diff --git a/sbs.conf b/sbs.conf
index 629c2d204b912372145b84bd1dc35fb3987d64ef..38d01d2e0d17fc33d6224c1b6d06e9a96e111322 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -73,6 +73,7 @@ ENTRY_PATH:       src/entrypoints
 TESTS_PATH:       src/tests
 SRC_PATH:         src/
 SBS_BASE:         skyward-boardcore
+PROJECT_INCLUDES: libs
 
 
 #-----------------------------------#
@@ -220,4 +221,12 @@ Include:	%shared
 Defines:   -DDEBUG
 Main:       drivers/hbridge-test
 
+[test-mavlink]
+Type:       test
+BoardId:    stm32f429zi_skyward_homeone
+BinName:    test-mavlink
+Include:	%shared %mavlink %gamma868
+Defines:   -DDEBUG -Ilibs
+Main:       drivers/test-mavlink
+
 
diff --git a/skyward-boardcore b/skyward-boardcore
index 9f1e23e3a29d2ac11349595832591a83ed4eba76..aa3ffe51911d52697d8085df1bcf018d9a657647 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit 9f1e23e3a29d2ac11349595832591a83ed4eba76
+Subproject commit aa3ffe51911d52697d8085df1bcf018d9a657647
diff --git a/src/tests/drivers/test-mavlink.cpp b/src/tests/drivers/test-mavlink.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd64ce81d0c0f01aca98c7d0f7518ad698ba00bd
--- /dev/null
+++ b/src/tests/drivers/test-mavlink.cpp
@@ -0,0 +1,79 @@
+/* Copyright (c) 2018 Skyward Experimental Rocketry
+ * Authors: Alvise de'Faveri Tron, Nuno Barcellos
+ *
+ * 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 <Common.h>
+#include <drivers/gamma868/Gamma868.h>
+#include "drivers/mavlink/MavManager.h"
+
+using namespace miosix;
+
+Gamma868* gamma868;
+MavManager* mavManager;
+
+static void onReceive(MavSender* sender, const mavlink_message_t& msg) 
+{
+    if (msg.msgid != MAVLINK_MSG_ID_ACK_TM) 
+    {
+        TRACE("[TmtcTest] Sending ack\n");
+
+        mavlink_message_t ackMsg;
+        mavlink_msg_ack_tm_pack(1, 1, &ackMsg, msg.msgid, msg.seq);
+
+        /* Send the message back to the sender */
+        bool ackSent = sender->enqueueMsg(ackMsg);
+
+        if(!ackSent)
+            TRACE("[Receiver] Could not enqueue ack\n");
+    }
+}
+
+int main()
+{
+    gamma868 = new Gamma868("/dev/radio");
+
+    mavManager = new MavManager();
+    mavManager->addSender(gamma868, 250);
+    mavManager->addReceiver(gamma868, mavManager->getSender(0), &onReceive);
+
+    while(1)
+    {
+        TRACE("[TmtcTest] Enqueueing ping\n");
+
+        // Create a Mavlink message
+        mavlink_message_t pingMsg;
+        mavlink_msg_ping_tc_pack(1, 1, &pingMsg, miosix::getTick());
+
+        // Send the message
+        bool ackSent = mavManager->getSender(0)->enqueueMsg(pingMsg);
+
+        if(!ackSent)
+            TRACE("[TmtcTest] Could not enqueue ping\n");
+
+        // ledOn();
+        // miosix::delayMs(200);
+        // ledOff();
+
+        miosix::Thread::sleep(1000);
+    }
+
+    return 0;
+}