diff --git a/Drivers/common/circular_buffer/src/circular_buffer.cpp b/Drivers/common/circular_buffer/src/circular_buffer.cpp
index c00d3197..18595b34 100644
--- a/Drivers/common/circular_buffer/src/circular_buffer.cpp
+++ b/Drivers/common/circular_buffer/src/circular_buffer.cpp
@@ -8,7 +8,10 @@ CircularBuffer::CircularBuffer(uint8_t* buf, uint16_t size) {
 }
 
 CircularBuffer::CircularBuffer(const CircularBuffer& other) {
-  
+    buf_ = other.buf_;
+    size_ = other.size_;
+    readPtr_ = other.readPtr_;
+    writePtr_ = other.writePtr_;
 }
 
 bool CircularBuffer::peek(uint8_t& byte, uint16_t index) {
diff --git a/SystemManager/Inc/SystemManager.hpp b/SystemManager/Inc/SystemManager.hpp
index 82c16dae..a9690e96 100644
--- a/SystemManager/Inc/SystemManager.hpp
+++ b/SystemManager/Inc/SystemManager.hpp
@@ -9,6 +9,7 @@
 #include "rcreceiver_datatypes.h"
 #include "independent_watchdog.h"
 #include "ZP_D_PWMChannel.hpp"
+#include "TelemetryManager.hpp"
 
 #define SBUS_THRESHOLD 25
 #define SBUS_MIN 0
@@ -20,6 +21,9 @@ class SystemManager {
     SystemManager();
     ~SystemManager();
 
+    /* Other managers*/
+    TelemetryManager *telemetryManager;
+
     /* Class Functions */
     void flyManually();
 
diff --git a/SystemManager/Src/SystemManager.cpp b/SystemManager/Src/SystemManager.cpp
index 365b5575..dc80723b 100644
--- a/SystemManager/Src/SystemManager.cpp
+++ b/SystemManager/Src/SystemManager.cpp
@@ -4,13 +4,15 @@
 //
 
 #include "SystemManager.hpp"
-#include "sbus_receiver.hpp"
-#include "sbus_defines.h"
-#include "rcreceiver_datatypes.h"
+
 #include "drivers_config.hpp"
 #include "independent_watchdog.h"
+#include "rcreceiver_datatypes.h"
+#include "sbus_defines.h"
+#include "sbus_receiver.hpp"
 #include "tim.h"
 
+
 #define TIMEOUT_CYCLES 250000 // 25k = 1 sec fro testing 10/14/2023 => 250k = 10 sec
 #define TIMOUT_MS      10000 // 10 sec
 
@@ -20,63 +22,86 @@ float prevyaw;
 float prevroll;
 float prevpitch;
 
-SystemManager::SystemManager():
-    rcController_(sbus_pointer),
-    throttleMotorChannel_(&htim2, TIM_CHANNEL_1),
-    yawMotorChannel_(&htim2, TIM_CHANNEL_2),
-    rollMotorChannel_(&htim2, TIM_CHANNEL_3),
-    pitchMotorChannel_(&htim2, TIM_CHANNEL_4),
-    invertedRollMotorChannel_(&htim3, TIM_CHANNEL_1),
-    watchdog_(TIMOUT_MS)
-{}
+SystemManager::SystemManager()
+    : rcController_(sbus_pointer),
+      throttleMotorChannel_(&htim2, TIM_CHANNEL_1),
+      yawMotorChannel_(&htim2, TIM_CHANNEL_2),
+      rollMotorChannel_(&htim2, TIM_CHANNEL_3),
+      pitchMotorChannel_(&htim2, TIM_CHANNEL_4),
+      invertedRollMotorChannel_(&htim3, TIM_CHANNEL_1),
+      watchdog_(TIMOUT_MS) {
+    // VARIABLES FOR TELEMETRY MANAGER TO HAVE AS REFERENCES THEY OBV SHOULD BE PUT SOMEWHERE ELSE,
+    // BUT I FEEL LIKE SM PM WOULD KNOW WHERE. MAYBE IN THE HPP FILE? IDK HOW YOU ARE PLANNING ON
+    // GATHERING THE DATA. I JUST PUT THEM HERE FOR NOW
+    int32_t lat = 0;
+    int32_t lon = 0;
+    int32_t alt = 0;
+    int32_t relative_alt = 0;
+    int16_t vx = 0;
+    int16_t vy = 0;
+    int16_t vz = 0;
+    uint16_t hdg = 0;
+    int32_t time_boot_ms = 0;
+    MAV_STATE state = MAV_STATE::MAV_STATE_STANDBY;
+    MAV_MODE_FLAG mode = MAV_MODE_FLAG::MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+    float roll = 0;
+    float pitch = 0;
+    float yaw = 0;
+    float rollspeed = 0;
+    float pitchspeed = 0;
+    float yawspeed = 0;
+
+    this->telemetryManager =
+        new TelemetryManager(lat, lon, alt, relative_alt, vx, vy, vz, hdg, time_boot_ms, state,
+                             mode, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
+    this->telemetryManager->init();
+    // IDK WHERE SM PLANS TO DO THIS, BUT telemetryManager.update() NEEDS TO BE CALLED AT A SEMI
+    // REGULAR INTERVAL AS IT DEALS WITH MESSAGE DECODING AND LOW PRIORITY DATA TRANSMISSION
+}
+
 
 SystemManager::~SystemManager() {}
 
 void SystemManager::flyManually() {
-    for(;;){
+    for (;;) {
         this->rcInputs_ = rcController_->GetRCControl();
 
-        //TO-DO: need to implement it using is_Data_New;
-        // boolean is true if data has not changed since the last cycle
-        bool is_unchanged{
-        	rcInputs_.throttle == prevthrottle &&
-        	rcInputs_.yaw == prevyaw &&
-			rcInputs_.roll == prevroll &&
-			rcInputs_.pitch == prevpitch
-        };
+        // TO-DO: need to implement it using is_Data_New;
+        //  boolean is true if data has not changed since the last cycle
+        bool is_unchanged{rcInputs_.throttle == prevthrottle && rcInputs_.yaw == prevyaw &&
+                          rcInputs_.roll == prevroll && rcInputs_.pitch == prevpitch};
 
         if (is_unchanged) {
-        	DisconnectionCount += 1; // if its not changed we increment the timeout counter
-        	if (DisconnectionCount > TIMEOUT_CYCLES) { // if timeout has occured
-        		DisconnectionCount = TIMEOUT_CYCLES+1; // avoid overflow but keep value above threshold
-        		this->rcInputs_.arm = 0; // failsafe
-        	}
+            DisconnectionCount += 1;  // if its not changed we increment the timeout counter
+            if (DisconnectionCount > TIMEOUT_CYCLES) {  // if timeout has occured
+                DisconnectionCount =
+                    TIMEOUT_CYCLES + 1;   // avoid overflow but keep value above threshold
+                this->rcInputs_.arm = 0;  // failsafe
+            }
         } else {
-        	DisconnectionCount = 0; //if the data has changed we want to reset out counter
+            DisconnectionCount = 0;  // if the data has changed we want to reset out counter
         }
 
-        watchdog_.refreshWatchdog(); // always hit the dog
+        watchdog_.refreshWatchdog();  // always hit the dog
 
-        if(this->rcInputs_.arm >= (SBUS_MAX/2)) {
-        		this->throttleMotorChannel_.set(rcInputs_.throttle);
-        		this->yawMotorChannel_.set(rcInputs_.yaw);
-        		this->rollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
-        		this->pitchMotorChannel_.set(SBUS_MAX - rcInputs_.pitch);
-        		this->invertedRollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
+        if (this->rcInputs_.arm >= (SBUS_MAX / 2)) {
+            this->throttleMotorChannel_.set(rcInputs_.throttle);
+            this->yawMotorChannel_.set(rcInputs_.yaw);
+            this->rollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
+            this->pitchMotorChannel_.set(SBUS_MAX - rcInputs_.pitch);
+            this->invertedRollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
 
-        		prevthrottle = rcInputs_.throttle;
-        		prevyaw = rcInputs_.yaw;
-        		prevroll = rcInputs_.roll;
-        		prevpitch = rcInputs_.pitch;
+            prevthrottle = rcInputs_.throttle;
+            prevyaw = rcInputs_.yaw;
+            prevroll = rcInputs_.roll;
+            prevpitch = rcInputs_.pitch;
 
-
-        }
-        else{
+        } else {
             this->throttleMotorChannel_.set(0);
-            this->yawMotorChannel_.set(SBUS_MAX/2);
-            this->rollMotorChannel_.set(SBUS_MAX/2);
-            this->pitchMotorChannel_.set(SBUS_MAX/2);
-            this->invertedRollMotorChannel_.set(SBUS_MAX/2);
+            this->yawMotorChannel_.set(SBUS_MAX / 2);
+            this->rollMotorChannel_.set(SBUS_MAX / 2);
+            this->pitchMotorChannel_.set(SBUS_MAX / 2);
+            this->invertedRollMotorChannel_.set(SBUS_MAX / 2);
         }
     }
 }
diff --git a/TelemetryManager/Inc/MavlinkDecoder.hpp b/TelemetryManager/Inc/MavlinkDecoder.hpp
new file mode 100644
index 00000000..11d31a71
--- /dev/null
+++ b/TelemetryManager/Inc/MavlinkDecoder.hpp
@@ -0,0 +1,83 @@
+/**
+ * @file MavlinkEncoder.hpp
+ * @brief This file is responsible for decoding MAVLink messages received from the ground station.
+ *
+ * @note Anything future maintainers should know about this file?
+ *
+ * @version Milestone 2
+ * @date 2023-08-24
+ * @author Yarema Dzulynsky: initial structure & implementation
+ *
+ * @warning Any issues you think are important/foresee in the future?
+ */
+
+#ifndef MAVLINKDECODER_H
+#define MAVLINKDECODER_H
+
+#include <functional>
+// #include <iostream>
+// #include <unordered_map>
+
+#include "Official_Mavlink_2_Library/common/mavlink.h"
+
+/**
+ * Macro for registering a decoder function for a specific message type.
+ * @param msgId (int) - the message id of the message type to be decoded.
+ * @param baseName (string) - the base name of the message type to be decoded. Ex. for
+ * mavlink_attitude_t, the base name is attitude. Essentially, whatever is in between mavlink_ and
+ * _t form the message you would like to register. The base name is used to generate the decoder
+ * function name. Ex. for mavlink_attitude_t, the decoder function name is
+ * mavlink_msg_attitude_decode.
+ * @param postProcessor ([](mavlink_**baseName**_t &message)) - the post processor function to be
+ * called after the message. This function must take in a reference to the message type that is
+ * being decoded. You can then use this reference to access the message fields. Ex. message.roll,
+ * message.pitch, etc. Now do what you want with them! :)
+ * @example REGISTER_DECODER(MAVLINK_MSG_ID_ATTITUDE, attitude, [](mavlink_attitude_t &message) {
+ *     //print out the message
+ *     std::cout << "ATTITUDE" << std::endl;
+ *     std::cout << "roll: " << message.roll << std::endl;
+ *     std::cout << "pitch: " << message.pitch << std::endl;
+ *     std::cout << "yaw: " << message.yaw << std::endl;
+ *     std::cout << "rollspeed: " << message.rollspeed << std::endl;
+ *     std::cout << "pitchspeed: " << message.pitchspeed << std::endl;
+ *     };
+ */
+// #define REGISTER_DECODER(msgId, baseName, postProcessor)    \
+//     decodingFunctions[msgId] = [](mavlink_message_t &msg) { \
+//         mavlink_##baseName##_t msgType;                     \
+//         mavlink_msg_##baseName##_decode(&msg, &msgType);    \
+//         postProcessor(msgType);                             \
+//     };
+
+class MavlinkDecoder {
+   public:
+    // The number of messages that have been decoded - used for testing purposes.
+    long messagesHandledSuccessfully = 0;
+
+    // std::unordered_map<int, std::function<void(mavlink_message_t &)>> decodingFunctions;
+    /**
+     * Default constructor.
+     */
+    MavlinkDecoder();
+
+    /**
+     * Default destructor.
+     */
+    ~MavlinkDecoder();
+
+    /**
+     * Unpacks the message, triggering the respective callback set in the switch statement.
+     * @param msg - The preformatted mavlink message, ready for decoding.
+     */
+
+    bool decodeMsg(mavlink_message_t &msg);
+
+    /**
+     * Transforms a byte sequence into mavlink messages, then activates the respective callbacks.
+     * @param buffer - Byte sequence for parsing.
+     * @param bufferSize - Length of the byte sequence.
+     */
+    void parseBytesToMavlinkMsgs(uint8_t *buffer, std::size_t bufferSize);
+};
+
+#endif
\ No newline at end of file
diff --git a/TelemetryManager/Inc/MavlinkEncoder.hpp b/TelemetryManager/Inc/MavlinkEncoder.hpp
new file mode 100644
index 00000000..efbfabb9
--- /dev/null
+++ b/TelemetryManager/Inc/MavlinkEncoder.hpp
@@ -0,0 +1,48 @@
+/**
+ * @file MavlinkEncoder.hpp
+ * @brief This file is responsible for encoding MAVLink messages to send to the ground station.
+ *
+ * @note Anything future maintainers should know about this file?
+ *
+ * @version Milestone 2
+ * @date 2023-08-24
+ * @author Yarema Dzulynsky: initial structure & implementation
+ *
+ * @warning Any issues you think are important/foresee in the future?
+ */
+
+#ifndef MAVLINKENCODER_H
+#define MAVLINKENCODER_H
+
+#include <iostream>
+
+#include "Official_Mavlink_2_Library/common/mavlink.h"
+#include "TMCircularBuffer.hpp"
+
+/**
+ * @brief Class dedicated to MAVLink message encoding.
+ *
+ * The 'MavlinkEncoder' class provides an interface and underlying functionality for MAVLink message
+ * encoding, using associated parameters and defined message types.
+ */
+class MavlinkEncoder {
+   public:
+    // The message object that's currently targeted for encoding.
+    mavlink_message_t currentMessage;
+
+    // Default constructor for the MavlinkEncoder class.
+    MavlinkEncoder();
+
+    // Default destructor for the MavlinkEncoder class.
+    ~MavlinkEncoder();
+
+    /**
+     * @brief Helper function to pack a mavlink message into a buffer.
+     *
+     * @param msg The mavlink message to be packed into the buffer.
+     * @param txToGroundByteQueue The buffer to pack the message into.
+     */
+    bool encodeMsgIntoBuffer(mavlink_message_t& msg, TMCircularBuffer& txToGroundByteQueue);
+};
+
+#endif
\ No newline at end of file
diff --git a/TelemetryManager/Inc/MavlinkTranslator.hpp b/TelemetryManager/Inc/MavlinkTranslator.hpp
index 8c3f3bf2..a094be91 100644
--- a/TelemetryManager/Inc/MavlinkTranslator.hpp
+++ b/TelemetryManager/Inc/MavlinkTranslator.hpp
@@ -1,23 +1,30 @@
 /**
  * @file MavlinkTranslator.hpp
- * @brief What does this file do?
+ * @brief This file is responsible for decoding/encoding MAVLink messages received_from/sent_to the ground station.
  *
  * @note Anything future maintainers should know about this file?
  *
- * @version 1.0
+ * @version Milestone 2
  * @date 2023-08-24
  * @author Yarema Dzulynsky: initial structure & implementation
  *
  * @warning Any issues you think are important/foresee in the future?
  */
 
-#include "TMCircularBuffer.hpp"
+#include "MavlinkDecoder.hpp"
+#include "MavlinkEncoder.hpp"
 #include "Official_Mavlink_2_Library/common/mavlink.h"
+#include "TMCircularBuffer.hpp"
 #ifndef MAVLINKTRANSLATOR_H
 #define MAVLINKTRANSLATOR_H
 
 class MavlinkTranslator {
-   public:
+      public:
+    // The decoder and encoder objects.
+    MavlinkDecoder decoder;
+    MavlinkEncoder encoder;
+    // The number of messages that have been decoded - used for testing purposes.
+    long decodedMessages = 0;
     /**
      * @brief Construct a new MavlinkTranslator object. Do whatever needs to be done here.
      *
@@ -47,8 +54,7 @@ class MavlinkTranslator {
      * @param txToGroundByteQueue The CircularBuffer containing the bytes to be sent to the ground
      * station.
      */
-    void addMavlinkMsgToByteQueue(mavlink_message_t &msg,
-                                          TMCircularBuffer &txToGroundByteQueue);
+    void addMavlinkMsgToByteQueue(mavlink_message_t &msg, TMCircularBuffer &txToGroundByteQueue);
 };
 
 #endif  // MAVLINKTRANSLATOR_H
diff --git a/TelemetryManager/Inc/TMCircularBuffer.hpp b/TelemetryManager/Inc/TMCircularBuffer.hpp
index 58badaa5..188d230f 100644
--- a/TelemetryManager/Inc/TMCircularBuffer.hpp
+++ b/TelemetryManager/Inc/TMCircularBuffer.hpp
@@ -4,7 +4,7 @@
  *
  * @note Anything future maintainers should know about this file?
  *
- * @version 1.0
+ * @version Milestone 2
  * @date 2023-08-24
  * @author Yarema Dzulynsky: initial structure
  * @author Rahul Ramkumar: implementation
@@ -19,20 +19,22 @@
 #include "circular_buffer.hpp"
 
 class TMCircularBuffer : public CircularBuffer {
-    using MAVLinkByte = unsigned char;  // not 100% sure if this is the right type
-
    public:
+    using MAVLinkByte = unsigned char;  // not 100% sure if this is the right type
+    long messagesInQueue = 0;
     /**
      * @brief Construct a new Circular Buffer object. Do whatever needs to be done here.
-     *
+     * @param buf The uint8_t buffer to be used by the TMCircularBuffer
+     * @param size The size of the buffer
      */
     TMCircularBuffer(uint8_t* buf, uint16_t size);
 
     /**
      * @brief Construct a new Circular Buffer object. Do whatever needs to be done here.
+     * @param buf The CircularBuffer base class object to be used by the TMCircularBuffer
      *
      */
-    TMCircularBuffer(CircularBuffer* buf );
+    TMCircularBuffer(CircularBuffer* buf);
 
     /**
      * @brief Destroy and cleanup memory (everything should be static anyways). Do whatever
@@ -44,41 +46,49 @@ class TMCircularBuffer : public CircularBuffer {
     /**
      * @brief Dequeue a byte from the queue
      *
+     * @param success A pointer to a boolean that will be set to true if the dequeue was successful
      * @return MAVLinkByte The byte that was dequeued
      */
-    MAVLinkByte dequeue();
+    MAVLinkByte dequeue(bool* success = nullptr);
 
     /**
      * @brief Enqueue a byte into the queue
      *
      * @param byte The byte to be enqueued
+     * 
+     * @return bool True if the byte was enqueued successfully, false otherwise
      */
     bool enqueue(MAVLinkByte byte);
 
     /**
-     * @brief Get the number of bytes until the end of the last full message in the queue
-     * determined by the end flag in the MAVLink message. This is so if we have a partial message 
-     * in the queue because an ISR was triggered while we were in the middle of enqueuing a message, 
-     * we only send completed messages and keep the partial message to be finished after the ISR.
-     * These partial messages once filled will be sent during the next transmission.
+     * @brief 
+     * 
+     * @param bytes The bytes to be enqueued
+     * @param size The number of bytes to be enqueued
+     * @return bool True if the bytes were enqueued successfully, false otherwise
+     */
+    bool enqueue(MAVLinkByte* bytes, uint16_t size);
+
+    /**
+     * @brief Get the number of bytes until the end of the first full message in the queue
+     * determined by the end flag in the MAVLink message. This is used to make sure we only
+     * send full messages.
+     * 
+     * @param success A pointer to a boolean that will be set to true if the operation was successful
+     * and false if it was not.
      *
-     * @return int The index of the last full message in the queue determined by the end flag
+     * @return int The number of bytes until the end of the first full message in the queue.
      * in the MAVLink message.
      *
      */
-    int bytesUntilLastMessageEnd();
+    int bytesUntilMessageEnd(bool* success = nullptr);
 
     /**
-     * @brief Returns the index of the current byte in the queue. This is useful for when we want to
-     * avoid sending partial messages, as we know the index of the end of the last complete message.
-     * Therefore, we can check if the current byte is just before the last complete message and if
-     * so, we can avoid sending it.
+     * @brief Returns the index of the current byte in the queue.
      *
      * @return int The index of the current byte in the queue.
      */
     uint16_t currentIndex();
-
-  
 };
 
 #endif
diff --git a/TelemetryManager/Inc/TelemetryManager.hpp b/TelemetryManager/Inc/TelemetryManager.hpp
index 906271fd..43aa435c 100644
--- a/TelemetryManager/Inc/TelemetryManager.hpp
+++ b/TelemetryManager/Inc/TelemetryManager.hpp
@@ -1,10 +1,12 @@
 /**
  * @file TelemetryManager.hpp
- * @brief What does this file do?
+ * @brief Manages multiple subclasses and FreeRTOS tasks that handle a 2 way communication link
+ * between the drone and the ground station and encoding and decoding Mavlink messages. This class
+ * essentially handles the link between the drone, other managers and Mission Planner.
  *
  * @note Anything future maintainers should know about this file?
  *
- * @version 1.0
+ * @version Milestone 2
  * @date 2023-08-24
  * @author Yarema Dzulynsky: initial structure & Implementation
  *
@@ -14,14 +16,75 @@
 #ifndef TELEMETRYMANAGER_H
 #define TELEMETRYMANAGER_H
 
+#include "FreeRTOS.h"
 #include "GroundStationCommunication.hpp"
 #include "MavlinkTranslator.hpp"
-#include "TelemetryTask.hpp"
+#include "task.h"
 
 class TelemetryManager {
-   private:
+   public:
     GroundStationCommunication GSC;
     MavlinkTranslator MT;
+    // the buffer that stores the bytes received from the ground station.
+    TMCircularBuffer* DMAReceiveBuffer;
+    // the buffer that stores non_routine/low_priority bytes (ex. Battery Voltage) to be sent to the
+    // ground station.
+    uint8_t* lowPriorityTransmitBuffer;
+    // the buffer that stores routine/high_priority bytes (ex. heading, general state data) to be
+    // sent to the ground station.
+    uint8_t* highPriorityTransmitBuffer;
+
+    /*References to variables that contain the state of the drone (lat, lng, yaw, pitch, etc..).
+     * They are updated by System Manager*/
+    // Altitude (MSL) (unit:
+    // mm)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+    int32_t& alt;
+    // The latitude of the drone (unit:
+    // degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+    int32_t& lat;
+    // The longitude of the drone (unit:
+    // degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+    int32_t& lon;
+    // Altitude above home (unit:
+    // mm)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+    int32_t& relative_alt;
+    // Ground X Speed (Latitude, positive north) (unit:
+    // cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+    int16_t& vx;
+    // Ground Y Speed (Longitude, positive east) (unit:
+    // cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+    int16_t& vy;
+    // Ground Z Speed (Altitude, positive down) (unit:
+    // cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+    int16_t& vz;
+    // Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
+    // (unit: cdeg)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+    uint16_t& hdg;
+    // Timestamp (time since system boot) (unit: ms)
+    // (https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+    int32_t& time_boot_ms;
+    // System status flag (https://mavlink.io/en/messages/minimal.html#MAV_STATE).
+    MAV_STATE& state;
+    // System mode bitmap (https://mavlink.io/en/messages/minimal.html#MAV_MODE_FLAG).
+    MAV_MODE_FLAG& mode;
+    // Roll angle (-pi..+pi) (unit: rad)
+    // (https://mavlink.io/en/messages/common.html#ATTITUDE).
+    float& roll;
+    // Pitch angle (-pi..+pi) (unit: rad)
+    // (https://mavlink.io/en/messages/common.html#ATTITUDE).
+    float& pitch;
+    // Yaw angle (-pi..+pi) (unit: rad)
+    // (https://mavlink.io/en/messages/common.html#ATTITUDE).
+    float& yaw;
+    // Roll angular speed (unit: rad/s)
+    // (https://mavlink.io/en/messages/common.html#ATTITUDE).
+    float& rollspeed;
+    // Pitch angular speed (unit: rad/s)
+    // (https://mavlink.io/en/messages/common.html#ATTITUDE).
+    float& pitchspeed;
+    // yawspeed Yaw angular speed (unit: rad/s)
+    // (https://mavlink.io/en/messages/common.html#ATTITUDE).
+    float& yawspeed;
 
     /**
      * @brief Create and configure FreeRTOS tasks.
@@ -35,12 +98,51 @@ class TelemetryManager {
      */
     void teardownTasks();
 
-   public:
     /**
      * @brief Construct a new Telemetry Manager object. Does not initialize the tasks.
      * To do so call the init() method.
+     * @param lat The latitude of the drone (unit:
+     * degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+     * @param lon The longitude of the drone(unit:
+     * degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+     * @param alt Altitude (MSL) (unit:
+     * mm)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+     * @param relative_alt Altitude above home (unit:
+     * mm)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+     * @param vx Ground X Speed (Latitude, positive north) (unit:
+     * cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+     * @param vy Ground Y Speed (Longitude, positive east) (unit:
+     * cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+     * @param vz Ground Z Speed (Altitude, positive down) (unit:
+     * cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+     * @param hdg Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
+     * (unit: cdeg)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+     * @param time_boot_ms Timestamp (time since system boot) (unit: ms)
+     * (https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT).
+     * @param state System status flag (https://mavlink.io/en/messages/minimal.html#MAV_STATE).
+     * @param mode System mode bitmap (https://mavlink.io/en/messages/minimal.html#MAV_MODE_FLAG).
+     * @param roll Roll angle (-pi..+pi) (unit: rad)
+     * (https://mavlink.io/en/messages/common.html#ATTITUDE).
+     * @param pitch Pitch angle (-pi..+pi) (unit: rad)
+     * (https://mavlink.io/en/messages/common.html#ATTITUDE).
+     * @param yaw Yaw angle (-pi..+pi) (unit: rad)
+     * (https://mavlink.io/en/messages/common.html#ATTITUDE).
+     * @param rollspeed Roll angular speed (unit: rad/s)
+     * (https://mavlink.io/en/messages/common.html#ATTITUDE).
+     * @param pitchspeed Pitch angular speed (unit: rad/s)
+     * (https://mavlink.io/en/messages/common.html#ATTITUDE).
+     * @param yawspeed Yaw angular speed (unit: rad/s)
+     * (https://mavlink.io/en/messages/common.html#ATTITUDE).
+     */
+    TelemetryManager(int32_t& lat, int32_t& lon, int32_t& alt, int32_t& relative_alt, int16_t& vx,
+                     int16_t& vy, int16_t& vz, uint16_t& hdg, int32_t& time_boot_ms,
+                     MAV_STATE& state, MAV_MODE_FLAG& mode, float& roll, float& pitch, float& yaw,
+                     float& rollspeed, float& pitchspeed, float& yawspeed);
+
+    /**
+     * @brief Destroy the Telemetry Manager object. Also destroys the FreeRTOS tasks associated with
+     * TM.
      */
-    TelemetryManager();
     ~TelemetryManager();
 
     /**
@@ -51,9 +153,8 @@ class TelemetryManager {
 
     /**
      * @brief Anything that is managed by SM should be updated here. This is basically the main loop
-     * of TM.
-     *
-     *
+     * of TM. Currently, it decodes MavLink messages from Missions Planner and packages and
+     * transmits non-routine and routine data to the ground station (like battery voltage etc).
      */
     void update();
 };
diff --git a/TelemetryManager/Inc/TelemetryTask.hpp b/TelemetryManager/Inc/TelemetryTask.hpp
deleted file mode 100644
index e35626ec..00000000
--- a/TelemetryManager/Inc/TelemetryTask.hpp
+++ /dev/null
@@ -1,67 +0,0 @@
-/**
- * @file TelemetryTask.hpp
- * @brief What does this file do?
- *
- * @note Anything future maintainers should know about this file?
- * - Don't touch TaskTrampoline. Took a while to get right and is confusing.
- *
- * @version 1.0
- * @date 2023-08-24
- * @author Yarema Dzulynsky: initial structure & implementation
- * @author Derek Tang: implementation
- *
- * @warning Any issues you think are important/foresee in the future?
- */
-
-#include <functional>
-
-#include "FreeRTOS.h"
-#include "TelemetryManager.hpp"
-#include "task.h"
-#ifndef TELEMETRYTASK_H
-#define TELEMETRYTASK_H
-
-class TelemetryTask {
-    // Simple alias for a lambda function that takes a TelemetryManager reference as an argument.
-    using Callback = std::function<void(TelemetryManager&)>;
-
-   private:
-    // TM instance reference to be used in the callback.
-    TelemetryManager& tm;
-    // Callback function to be called when the timer interrupt is triggered.
-    Callback cbLambda;
-    // Handle to the task. Or should this be something else? Since we are doing a timer interrupt?
-    TaskHandle_t xHandle;
-
-    /**
-     * @brief This is essentially a compatibility/wrapper function that allows us to use a lambda
-     * function which has a class instance as an argument as a callback for the timer interrupt.
-     * This allows us to access TM within the lambda function.
-     *
-     * @param pvParameters - The TelemetryTask object.
-     */
-    static void TaskTrampoline(void* pvParameters);
-
-   public:
-    /**
-     * @brief Construct a new Timer TelemetryTask object which will call the given lambda function
-     * at the given interval. This will allow easier scheduling of data transfer or any TM tasks
-     * which need to happen at a regular interval with FreeRTOS.
-     *
-     * @param taskName - The name of the task.
-     * @param stackSize - The size of the stack for the task.
-     * @param uxPriority - The priority of the task.
-     * @param tm - The TelemetryManager object.
-     * @param cbLambda - The callback function to be called when the timer interrupt is triggered.
-     */
-    TelemetryTask(const char* taskName, int stackSize, UBaseType_t uxPriority, TelemetryManager& tm,
-                  Callback cbLambda);
-
-    /**
-     * @brief Destroy the Timer Interrupt object.
-     *
-     */
-    ~TelemetryTask();
-};
-
-#endif
diff --git a/TelemetryManager/Src/GroundStationCommunication.cpp b/TelemetryManager/Src/GroundStationCommunication.cpp
index be870fa3..77a3d302 100644
--- a/TelemetryManager/Src/GroundStationCommunication.cpp
+++ b/TelemetryManager/Src/GroundStationCommunication.cpp
@@ -4,34 +4,40 @@
 
 #include "drivers_config.hpp"
 
-GroundStationCommunication::GroundStationCommunication(
-    TMCircularBuffer& DMAReceiveBuffer, uint8_t *lowPriorityTransmitBuffer,
-    uint8_t *highPriorityTransmitBuffer, int length):
-    DMAReceiveBuffer(DMAReceiveBuffer),
-    lowPriorityTransmitBuffer(lowPriorityTransmitBuffer, length),
-    highPriorityTransmitBuffer(highPriorityTransmitBuffer, length) {
-
-            
-}
+GroundStationCommunication::GroundStationCommunication(TMCircularBuffer &DMAReceiveBuffer,
+                                                       uint8_t *lowPriorityTransmitBuffer,
+                                                       uint8_t *highPriorityTransmitBuffer,
+                                                       int length)
+    : DMAReceiveBuffer(DMAReceiveBuffer),
+      lowPriorityTransmitBuffer(lowPriorityTransmitBuffer, length),
+      highPriorityTransmitBuffer(highPriorityTransmitBuffer, length) {}
 
 GroundStationCommunication::~GroundStationCommunication() {
     // Destructor
 }
 
-// ** Implement transmit first **
-
 void GroundStationCommunication::transmit(TMCircularBuffer &transmissionBuffer) {
     // START: Send the bytes in transmissionBuffer to the ground station via RFD900
-    int bytesToTransmit = transmissionBuffer.bytesUntilLastMessageEnd();
-    
-    if (bytesToTransmit > RFD900_BUF_SIZE) {
-        bytesToTransmit = RFD900_BUF_SIZE;
-    }
 
-    for (int i {0}; i < bytesToTransmit; ++i) {
-        internalBuffer_[i] = transmissionBuffer.dequeue();
+    // While there are still messages in the transmissionBuffer, send em
+    while (transmissionBuffer.messagesInQueue >= 0) {
+        int bytesToTransmit = transmissionBuffer.bytesUntilMessageEnd();
+
+        // Make sure we don't overflow the buffer
+        if (bytesToTransmit > RFD900_BUF_SIZE) {
+            bytesToTransmit = RFD900_BUF_SIZE;
+        }
+
+        // Dequeue the bytes from the transmissionBuffer and put them in the internal buffer
+        for (int i{0}; i < bytesToTransmit; ++i) {
+            internalBuffer_[i] = transmissionBuffer.dequeue();
+        }
+        // Send the bytes in the internal buffer to the ground station via RFD900
+        pRFD900->transmit(internalBuffer_, bytesToTransmit);
+
+        // Decrement the number of messages in the transmissionBuffer
+        transmissionBuffer.messagesInQueue--;
     }
-    pRFD900->transmit(internalBuffer_, bytesToTransmit);
 
     // END: Send the bytes in transmissionBuffer to the ground station via RFD900
 
diff --git a/TelemetryManager/Src/MavlinkDecoder.cpp b/TelemetryManager/Src/MavlinkDecoder.cpp
new file mode 100644
index 00000000..e4738a30
--- /dev/null
+++ b/TelemetryManager/Src/MavlinkDecoder.cpp
@@ -0,0 +1,86 @@
+//
+// Created by Yarema Dzulynsky on 2023-07-10.
+//
+
+#include "MavlinkDecoder.hpp"
+
+MavlinkDecoder::MavlinkDecoder() {}
+
+MavlinkDecoder::~MavlinkDecoder() {
+    // no cleanup needed
+}
+
+void MavlinkDecoder::parseBytesToMavlinkMsgs(uint8_t *buffer, std::size_t bufferSize) {
+    // Process the bytes in the buffer to construct MAVLink messages
+
+    // MAVLink message structure
+    mavlink_message_t currentMessage;
+
+    // Status of the MAVLink message decoding process
+    mavlink_status_t currentMessageDecodingStatus;
+
+    std::size_t currentBufferIndex = 0;  // Index to traverse the buffer
+
+    while (currentBufferIndex < bufferSize) {
+        // Try to parse the current byte and see if it corresponds to a MAVLink message
+        if (mavlink_parse_char(MAVLINK_COMM_0, buffer[currentBufferIndex], &currentMessage,
+                               &currentMessageDecodingStatus)) {
+            // Attempt to decode the constructed MAVLink message
+            bool isMessageDecoded = decodeMsg(currentMessage);
+        }
+        currentBufferIndex++;
+    }
+}
+
+// must return true if the message was handled successfully
+bool handle_attitude(mavlink_message_t &msg) {
+    mavlink_attitude_t payload;
+    mavlink_msg_attitude_decode(&msg, &payload);
+
+    // do what you want with the payload
+
+    return true;
+}
+
+// must return true if the message was handled successfully
+bool handle_global_position_int(mavlink_message_t &msg) {
+    mavlink_global_position_int_t payload;
+    mavlink_msg_global_position_int_decode(&msg, &payload);
+
+    // do what you want with the payload
+    return true;
+}
+
+// must return true if the message was handled successfully
+bool handle_heartbeat(mavlink_message_t &msg) {
+    mavlink_heartbeat_t payload;
+    mavlink_msg_heartbeat_decode(&msg, &payload);
+
+    // do what you want with the payload
+    return true;
+}
+
+bool MavlinkDecoder::decodeMsg(mavlink_message_t &msg) {
+    int messageId = msg.msgid;  // Extract message ID
+    bool handledSuccessfully = false;
+    switch (messageId) {
+        case MAVLINK_MSG_ID_ATTITUDE:
+            handledSuccessfully = handle_attitude(msg);
+            handledSuccessfully ? messagesHandledSuccessfully++ : messagesHandledSuccessfully;
+
+            break;
+        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
+            handledSuccessfully = handle_global_position_int(msg);
+            handledSuccessfully ? messagesHandledSuccessfully++ : messagesHandledSuccessfully;
+
+            break;
+        case MAVLINK_MSG_ID_HEARTBEAT:
+            handledSuccessfully = handle_heartbeat(msg);
+            handledSuccessfully ? messagesHandledSuccessfully++ : messagesHandledSuccessfully;
+
+            break;
+        default:
+            break;
+    }
+    return handledSuccessfully;
+}
diff --git a/TelemetryManager/Src/MavlinkEncoder.cpp b/TelemetryManager/Src/MavlinkEncoder.cpp
new file mode 100644
index 00000000..968b7aca
--- /dev/null
+++ b/TelemetryManager/Src/MavlinkEncoder.cpp
@@ -0,0 +1,23 @@
+// This file was created by Yarema Dzulynsky on 2023-07-24.
+#include "MavlinkEncoder.hpp"
+
+// Default constructor for the MavlinkEncoder class.
+MavlinkEncoder::MavlinkEncoder() = default;
+
+// Default destructor for the MavlinkEncoder class.
+MavlinkEncoder::~MavlinkEncoder() = default;
+
+/**
+ * @brief Packs a mavlink message into a buffer.
+ * 
+ * @param msg The Mavlink message to be packed into the buffer.
+ * @param txToGroundByteQueue The buffer to pack the message into.
+ * @return bool True if the message was successfully packed into the buffer, false otherwise. 
+ */
+bool MavlinkEncoder::encodeMsgIntoBuffer(mavlink_message_t &msg,
+                                      TMCircularBuffer &txToGroundByteQueue) {
+    uint8_t messageBuffer[MAVLINK_MAX_PACKET_LEN] = {0};
+    size_t messageBufferLen = mavlink_msg_to_send_buffer(messageBuffer, &msg);
+    bool success = txToGroundByteQueue.enqueue(messageBuffer, messageBufferLen);
+    return success;
+}
diff --git a/TelemetryManager/Src/MavlinkTranslator.cpp b/TelemetryManager/Src/MavlinkTranslator.cpp
index 4087f604..52f36453 100644
--- a/TelemetryManager/Src/MavlinkTranslator.cpp
+++ b/TelemetryManager/Src/MavlinkTranslator.cpp
@@ -1,23 +1,37 @@
 #include "MavlinkTranslator.hpp"
 
-MavlinkTranslator::MavlinkTranslator()
-{
+MavlinkTranslator::MavlinkTranslator() {
     // Constructor
 }
 
-MavlinkTranslator::~MavlinkTranslator()
-{
+MavlinkTranslator::~MavlinkTranslator() {
     // Destructor
 }
 
-void MavlinkTranslator::bytesToMavlinkMsg(TMCircularBuffer &rxFromGroundByteQueue)
-{
-
+void MavlinkTranslator::bytesToMavlinkMsg(TMCircularBuffer &rxFromGroundByteQueue) {
     // Take bytes in rxFromGroundByteQueue and convert them to Mavlink messages
-}
+    bool success = true;
+    uint8_t bytesUntilEnd = 0;
+
+    while (success) {
+        bytesUntilEnd = rxFromGroundByteQueue.bytesUntilMessageEnd(&success);
 
-void MavlinkTranslator::addMavlinkMsgToByteQueue(mavlink_message_t &msg, TMCircularBuffer &txToGroundByteQueue)
-{
+        if (success) {
+            if (bytesUntilEnd != 0) {
+                uint8_t rxByte[bytesUntilEnd];
+
+                for (uint8_t i = 0; i < bytesUntilEnd; i++) {
+                    rxByte[i] = rxFromGroundByteQueue.dequeue();
+                }
+                decoder.parseBytesToMavlinkMsgs(rxByte, bytesUntilEnd);
+                decodedMessages++;
+            }
+        }
+    }
+}
 
-    // Take Mavlink messages in txToGroundByteQueue and convert them to bytes
+void MavlinkTranslator::addMavlinkMsgToByteQueue(mavlink_message_t &msg,
+                                                 TMCircularBuffer &txToGroundByteQueue) {
+    encoder.encodeMsgIntoBuffer(msg, txToGroundByteQueue);
+    txToGroundByteQueue.messagesInQueue++;
 }
\ No newline at end of file
diff --git a/TelemetryManager/Src/TMCircularBuffer.cpp b/TelemetryManager/Src/TMCircularBuffer.cpp
index 414dcc7a..d087ef46 100644
--- a/TelemetryManager/Src/TMCircularBuffer.cpp
+++ b/TelemetryManager/Src/TMCircularBuffer.cpp
@@ -1,55 +1,71 @@
 
 #include "TMCircularBuffer.hpp"
 
-TMCircularBuffer::TMCircularBuffer(uint8_t* buf, uint16_t size): CircularBuffer(*buf, size) {
+TMCircularBuffer::TMCircularBuffer(uint8_t* buf, uint16_t size) : CircularBuffer(buf, size) {
     // Constructor
 }
 
-TMCircularBuffer::TMCircularBuffer(CircularBuffer* buf ): CircularBuffer(*buf) {
+TMCircularBuffer::TMCircularBuffer(CircularBuffer* buf) : CircularBuffer(*buf) {
     // Constructor
- }
+}
 
 TMCircularBuffer::~TMCircularBuffer() {
     // Destructor
-   
- }
+}
 
-TMCircularBuffer::MAVLinkByte  TMCircularBuffer::dequeue() {
-    MAVLinkByte* res;
-    if(read(res,1)){
-        return *res;
+TMCircularBuffer::MAVLinkByte TMCircularBuffer::dequeue(bool* success) {
+    MAVLinkByte res = 0;
+    if (read(&res, 1)) {
+        if (success != nullptr) {
+            *success = true;
+        }
+        return res;
+    }
+    if (success != nullptr) {
+        *success = false;
     }
     return 0;
 }
 
 bool TMCircularBuffer::enqueue(MAVLinkByte byte) {
     // Enqueue the byte
-    if(write(byte)){
+    if (write(byte)) {
         return true;
     }
-     return false;
+    return false;
 }
 
-int TMCircularBuffer::bytesUntilLastMessageEnd() {
-    /*
-    Rahul: This one is a bit tricky because you need to know the structure of the MAVLink message. 
-    I can help you with this one if you want.
-    */
-   int index = -1;
-   uint16_t count = 0;
-   uint16_t size = size_;
-   while(count < size){
-      if( buf_[index] == 0xFD){
-        return index;
-      }
-      
-      index = (index + 1) % size;
-      count ++;
-   }
-   return index;
-   
+bool TMCircularBuffer::enqueue(MAVLinkByte* bytes, uint16_t size) {
+    for (uint16_t i = 0; i < size; i++) {
+        if (!enqueue(bytes[i])) {
+            return false;
+        }
+    }
+    return true;
+}
+
+int TMCircularBuffer::bytesUntilMessageEnd(bool* success) {
+    int index = 0;
+    uint8_t byte = 0;
+    uint16_t size = writePtr_ - readPtr_;
+
+    while (index < size) {
+        if (peek(byte, index)) {
+            if (byte == 0xFD) {  // 0xFD is the start flag for a MAVLink message
+                if (success != nullptr) {
+                    *success = true;
+                }
+                if (index != 0) {
+                    return index;
+                }
+            }
+        }
+        index++;
+    }
+    if (success != nullptr) {
+        *success = false;
+    }
+    return index;
 }
 
-uint16_t TMCircularBuffer::currentIndex() {
-    return readPtr_;
- }
+uint16_t TMCircularBuffer::currentIndex() { return readPtr_; }
diff --git a/TelemetryManager/Src/TelemetryManager.cpp b/TelemetryManager/Src/TelemetryManager.cpp
index 0c5996d1..b885cc42 100644
--- a/TelemetryManager/Src/TelemetryManager.cpp
+++ b/TelemetryManager/Src/TelemetryManager.cpp
@@ -1,19 +1,36 @@
-#include "TelemetryManager.hpp"
 
-/**
- * @brief This task is called every 500ms. It is responsible for
- * sending the highest priority/routine drone "state" data to the ground station. Data such as
- * heartbeat message, altitude, attitude, latitude, longitude... And anything else deemed
- * important enough to be transmitted at a regular interval. This is the highest priority
- * data in the GSC.highPriorityTransmitBuffer.
- *
- */
-TelemetryTask* routineDataTransmission;
+#include "TelemetryManager.hpp"
 
-TelemetryManager::TelemetryManager() {
-    this->MT = MavlinkTranslator();
-    this->GSC = GroundStationCommunication();
-}
+// FreeRTOS task handle for the routineDataTransmission task
+TaskHandle_t routineDataTransmissionH = NULL;
+
+TelemetryManager::TelemetryManager(int32_t& lat, int32_t& lon, int32_t& alt, int32_t& relative_alt,
+                                   int16_t& vx, int16_t& vy, int16_t& vz, uint16_t& hdg,
+                                   int32_t& time_boot_ms, MAV_STATE& state, MAV_MODE_FLAG& mode,
+                                   float& roll, float& pitch, float& yaw, float& rollspeed,
+                                   float& pitchspeed, float& yawspeed)
+    : DMAReceiveBuffer(new TMCircularBuffer(rfd900_circular_buffer)),
+      lowPriorityTransmitBuffer(new uint8_t[RFD900_BUF_SIZE]),
+      highPriorityTransmitBuffer(new uint8_t[RFD900_BUF_SIZE]),
+      GSC(*DMAReceiveBuffer, lowPriorityTransmitBuffer, highPriorityTransmitBuffer,
+          RFD900_BUF_SIZE),
+      lat(lat),
+      lon(lon),
+      alt(alt),
+      relative_alt(relative_alt),
+      vx(vx),
+      vy(vy),
+      vz(vz),
+      hdg(hdg),
+      time_boot_ms(time_boot_ms),
+      state(state),
+      mode(mode),
+      roll(roll),
+      pitch(pitch),
+      yaw(yaw),
+      rollspeed(rollspeed),
+      pitchspeed(pitchspeed),
+      yawspeed(yawspeed) {}
 
 TelemetryManager::~TelemetryManager() {
     // Destructor
@@ -21,7 +38,7 @@ TelemetryManager::~TelemetryManager() {
 }
 
 /**
- * @brief Initialize TM threads and timer interrupts.
+ * @brief Initialize TM tasks (and other later).
  *
  */
 void TelemetryManager::init() {
@@ -29,54 +46,116 @@ void TelemetryManager::init() {
     spinUpTasks();
 }
 
+/**
+ * @brief This FreeRTOS task executes the code in the while loop every 500ms. It is responsible for
+ * sending the highest priority/routine drone "state" data to the ground station. Data such as
+ * heartbeat message, altitude, attitude, latitude, longitude... And anything else deemed
+ * important enough to be transmitted at a regular interval. This is the highest priority
+ * data in the GSC.highPriorityTransmitBuffer.
+ */
+void routineDataTransmission(void* pvParameters) {
+    // placeholder for now
+    uint8_t system_id = 0;
+    // placeholder for now
+    uint8_t component_id = 0;
+
+    // for diagnostic purposes
+    bool greenOn = false;
+    bool blueOn = false;
+    bool redOn = false;
+
+    /*
+    Cast the void pointer to a TelemetryManager pointer so we
+    can access the TelemetryManager object's members.
+    */
+    auto* tm = static_cast<TelemetryManager*>(pvParameters);
+
+    while (1) {
+        /* For diagnostic purposes, toggle the green and blue LEDs on the Nucleo board
+        HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin,
+                          greenOn ? GPIO_PIN_RESET : GPIO_PIN_SET);
+        HAL_GPIO_WritePin(LED_BLUE_GPIO_Port, LED_BLUE_Pin, blueOn ? GPIO_PIN_RESET : GPIO_PIN_SET);
+
+        greenOn = !greenOn;
+        blueOn = !blueOn;
+        */
+
+        // START: ingest drone state data and pack bytes into GSC.highPriorityTransmitBuffer
+
+        /* Create a global_position_int message(lat lon alt relative_alt vx vy vz hdg - general
+         * spatial data)*/
+        mavlink_message_t globalPositionIntMsg = {0};
+
+        // Pack the message with the actual data
+        mavlink_msg_global_position_int_pack(system_id, component_id, &globalPositionIntMsg,
+                                             tm->time_boot_ms, tm->lat, tm->lon, tm->alt,
+                                             tm->relative_alt, tm->vx, tm->vy, tm->vz, tm->hdg);
+
+        // Add the packed message to the byte queue for later transmission
+        tm->MT.addMavlinkMsgToByteQueue(globalPositionIntMsg, tm->GSC.highPriorityTransmitBuffer);
+
+        // Create an attitude message
+        mavlink_message_t attitudeMsg = {0};
+        // Pack the message with the actual data
+        mavlink_msg_attitude_pack(system_id, component_id, &attitudeMsg, tm->time_boot_ms, tm->roll,
+                                  tm->pitch, tm->yaw, tm->rollspeed, tm->pitchspeed, tm->yawspeed);
+        // Add the packed message to the byte queue for later transmission
+        tm->MT.addMavlinkMsgToByteQueue(attitudeMsg, tm->GSC.highPriorityTransmitBuffer);
+
+        /* END: ingest drone state data and pack bytes into GSC.highPriorityTransmitBuffer*/
+
+        // Create a heartbeat message
+        mavlink_message_t heartbeatMsg = {0};
+        // Pack the message with the actual data
+        mavlink_msg_heartbeat_pack(system_id, component_id, &heartbeatMsg, MAV_TYPE_QUADROTOR,
+                                   MAV_AUTOPILOT_INVALID, tm->mode, 0, tm->state);
+        // Add the packed message to the byte queue for later transmission
+        tm->MT.addMavlinkMsgToByteQueue(heartbeatMsg, tm->GSC.highPriorityTransmitBuffer);
+
+        /*NOTE: The TelemetryManager object's members such as lat, lon, alt, relative_alt, vx, vy,
+           vz etc are references to variables that are updated by SystemManager. Which is how they
+           stay up to date
+           */
+
+        // transmit the high priority data to the ground station
+        tm->GSC.transmit(tm->GSC.highPriorityTransmitBuffer);
+
+        // The delay between each state data packing and transmission
+        vTaskDelay(pdMS_TO_TICKS(500));  // Adjust the delay as necessary
+    }
+}
 void TelemetryManager::spinUpTasks() {
-    routineDataTransmission =
-        new TelemetryTask("routineDataTransmission", configMINIMAL_STACK_SIZE, tskIDLE_PRIORITY,
-                          *this, [](TelemetryManager& tm) -> void {
-                              auto GSC = tm.GSC;
-                              while (true) {
-                                  // START: ingest drone state data and pack bytes into
-                                  // GSC.highPriorityTransmitBuffer
-
-                                  // END: ingest drone state data and pack bytes into
-                                  // GSC.highPriorityTransmitBuffer
-
-                                  // transmit the data via GSC.transmit(); function
-                                  GSC.transmit(GSC.highPriorityTransmitBuffer);
-
-                                  // The interval at which the instructions in the lambda function
-                                  // are executed.
-                                  vTaskDelay(pdMS_TO_TICKS(500));  // Adjust the delay as necessary
-                              }
-                          });
-                          
+    // Create a task that will send state data to the ground station every 500ms.
+    xTaskCreate(&routineDataTransmission, "routineDataTransmission", 512UL, this, 24,
+                &routineDataTransmissionH);
 }
 
 void TelemetryManager::update() {
-    /* 
-     * @brief the following code up to END is responsible for taking data from other managers and converting
-     * them to Mavlink bytes, then putting them into GSC.lowPriorityTransmitBuffer.
+    /*
+     * @brief the following code up to END is responsible for taking data from other managers
+     * and converting them to Mavlink bytes, then putting them into
+     * GSC.lowPriorityTransmitBuffer.
      */
 
     // START: fill GSC.lowPriorityTransmitBuffer with data to transmit
 
     // END: fill GSC.lowPriorityTransmitBuffer with data to transmit
 
-
-
-
     /*
-     * the following code up to END is responsible for taking the bytes from the GSC.DMAReceiveBuffer and
-     * converting them to Mavlink messages/triggering the callbacks associated with each Mavlink
-     * message.
+     * the following code up to END is responsible for taking the bytes from the
+     * GSC.DMAReceiveBuffer and converting them to Mavlink messages/triggering the callbacks
+     * associated with each Mavlink message.
      */
-    //START: convert bytes from GSC.DMAReceiveBuffer to Mavlink messages
+    // START: convert bytes from GSC.DMAReceiveBuffer to Mavlink messages
     MT.bytesToMavlinkMsg(GSC.DMAReceiveBuffer);
-    //END: convert bytes from GSC.DMAReceiveBuffer to Mavlink messages
-
+    // END: convert bytes from GSC.DMAReceiveBuffer to Mavlink messages
 
     // transmit non routine data via GSC.transmit(); function
     GSC.transmit(GSC.lowPriorityTransmitBuffer);
 }
 
-void TelemetryManager::teardownTasks() { delete routineDataTransmission; }
+void TelemetryManager::teardownTasks() {
+    if (eTaskGetState(routineDataTransmissionH) != eDeleted) {
+        vTaskDelete(routineDataTransmissionH);
+    }
+}
diff --git a/TelemetryManager/Src/TelemetryTask.cpp b/TelemetryManager/Src/TelemetryTask.cpp
deleted file mode 100644
index 34bed4aa..00000000
--- a/TelemetryManager/Src/TelemetryTask.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-
-#include "TelemetryTask.hpp"
-
-TelemetryTask::TelemetryTask(const char *taskName, int stackSize, UBaseType_t uxPriority, TelemetryManager &tm, Callback cbLambda)
-    : tm(tm), cbLambda(cbLambda), xHandle(nullptr) {
-    xTaskCreate(&TelemetryTask::TaskTrampoline, taskName, stackSize, this, uxPriority, &xHandle);
-}
-
-void TelemetryTask::TaskTrampoline(void *pvParameters) {
-        auto* context = static_cast<TelemetryTask*>(pvParameters);
-        context->cbLambda(context->tm);
-    }
-
-TelemetryTask::~TelemetryTask() {
-    if (eTaskGetState(xHandle) != eDeleted) {
-        vTaskDelete(xHandle);
-    }
-}
diff --git a/TelemetryManager/Tests/MavlinkTranslatorTest.cpp b/TelemetryManager/Tests/MavlinkTranslatorTest.cpp
new file mode 100644
index 00000000..8bb59e35
--- /dev/null
+++ b/TelemetryManager/Tests/MavlinkTranslatorTest.cpp
@@ -0,0 +1,84 @@
+#include <gtest/gtest.h>
+
+#include <MavlinkTranslator.hpp>
+
+TEST(MavlinkTranslatorTest, EncodeThenDecodeTest) {
+    MavlinkTranslator translator;
+
+    mavlink_message_t globalPositionInt;
+    mavlink_message_t attitude;
+    mavlink_msg_global_position_int_pack(0, 0, &globalPositionInt, 0, 1, 2, 3, 0, 7, 8, 9, 0);
+    mavlink_msg_attitude_pack(0, 0, &attitude, 0, 4, 5, 6, 0, 0, 0);
+
+    uint8_t encoderBuffer[2 * MAVLINK_MAX_PACKET_LEN] = {0};
+    TMCircularBuffer buffer(encoderBuffer, 2 * MAVLINK_MAX_PACKET_LEN);
+
+    translator.addMavlinkMsgToByteQueue(globalPositionInt, buffer);
+    translator.addMavlinkMsgToByteQueue(attitude, buffer);
+
+    // for (size_t i = 0; i < actDataSize; i++) {
+    //     buffer.enqueue(encoderBuffer[i]);
+    // }
+
+    int expectedNumberOfMessagesDecoded =
+        1;  // should be one not two because we want to wait for the next message to determine the
+            // start bit and therefore determine when the second message ends
+
+    translator.bytesToMavlinkMsg(buffer);
+
+    ASSERT_EQ(translator.decoder.messagesHandledSuccessfully, expectedNumberOfMessagesDecoded);
+    ASSERT_EQ(translator.decodedMessages, expectedNumberOfMessagesDecoded);
+}
+
+TEST(MavlinkTranslatorTest, ParseBytesToMavlinkMsgsTest) {
+    MavlinkTranslator translator;
+    int expectedNumberOfMessagesDecoded = 1;
+
+    mavlink_message_t globalPositionIntMsg, attitudeMsg;
+    uint8_t globalPositionIntBuffer[MAVLINK_MAX_PACKET_LEN] = {0};
+    uint8_t attitudeBuffer[MAVLINK_MAX_PACKET_LEN] = {0};
+
+    mavlink_msg_global_position_int_pack(0, 0, &globalPositionIntMsg, 0, 1, 2, 3, 0, 7, 8, 9, 0);
+    size_t globalPositionIntBufferLen =
+        mavlink_msg_to_send_buffer(globalPositionIntBuffer, &globalPositionIntMsg);
+
+    mavlink_msg_attitude_pack(0, 0, &attitudeMsg, 0, 4, 5, 6, 0, 0, 0);
+    size_t attitudeBufferLen = mavlink_msg_to_send_buffer(attitudeBuffer, &attitudeMsg);
+
+    uint8_t outputBuffer[MAVLINK_MAX_PACKET_LEN * 2] = {0};
+
+    memcpy(outputBuffer, globalPositionIntBuffer, globalPositionIntBufferLen);
+
+    memcpy(outputBuffer + globalPositionIntBufferLen, attitudeBuffer, attitudeBufferLen);
+
+    TMCircularBuffer buffer(outputBuffer, globalPositionIntBufferLen + attitudeBufferLen);
+
+    buffer.enqueue(outputBuffer, globalPositionIntBufferLen + attitudeBufferLen);
+
+    translator.bytesToMavlinkMsg(buffer);
+
+    ASSERT_EQ(translator.decoder.messagesHandledSuccessfully, expectedNumberOfMessagesDecoded);
+    ASSERT_EQ(translator.decodedMessages, expectedNumberOfMessagesDecoded);
+}
+
+TEST(MavlinkTranslatorTest, AddMavlinkMsgToByteQueueTest) {
+    MavlinkTranslator translator;
+    uint8_t _buffer[256];
+    TMCircularBuffer buffer(_buffer, 256);
+
+    // Create a mavlink message
+    mavlink_message_t msg;
+    mavlink_msg_global_position_int_pack(0, 0, &msg, 0, 1, 2, 3, 0, 7, 8, 9, 0);
+
+    // Add the message to the byte queue
+    translator.addMavlinkMsgToByteQueue(msg, buffer);
+
+    // Create a buffer to hold the message data
+    uint8_t expectedBuffer[MAVLINK_MAX_PACKET_LEN] = {0};
+    size_t expectedBufferLen = mavlink_msg_to_send_buffer(expectedBuffer, &msg);
+
+    // Verify the data in the byte queue matches the expected buffer
+    for (size_t i = 0; i < expectedBufferLen; i++) {
+        ASSERT_EQ(buffer.dequeue(), expectedBuffer[i]);
+    }
+}
\ No newline at end of file
diff --git a/TelemetryManager/Tests/TMCircularBufferTest.cpp b/TelemetryManager/Tests/TMCircularBufferTest.cpp
new file mode 100644
index 00000000..25c348f8
--- /dev/null
+++ b/TelemetryManager/Tests/TMCircularBufferTest.cpp
@@ -0,0 +1,113 @@
+#include <gtest/gtest.h>
+
+#include "TMCircularBuffer.hpp"
+
+TEST(TMCircularBufferTest, EnqueueDequeue) {
+    static const uint16_t bufferSize = MAX_MAVLINK_MSG_SIZE;
+    uint8_t buffer[bufferSize];
+    TMCircularBuffer circularBuffer(buffer, bufferSize);
+
+    TMCircularBuffer::MAVLinkByte byte = 0x42;  // Arbitrary byte value
+    EXPECT_TRUE(circularBuffer.enqueue(byte));
+    EXPECT_EQ(circularBuffer.dequeue(), byte);
+}
+
+TEST(TMCircularBufferTest, DequeueEmptyBuffer) {
+    static const uint16_t bufferSize = 0;
+    uint8_t buffer[bufferSize];
+    TMCircularBuffer circularBuffer(buffer, bufferSize);
+    bool success = true;
+    circularBuffer.dequeue(&success);
+
+    EXPECT_FALSE(success);
+}
+
+TEST(TMCircularBufferTest, BytesUntilLastMessageEndEmptyBuffer) {  // failing
+    static const uint16_t bufferSize = 0;
+    uint8_t buffer[bufferSize];
+    TMCircularBuffer circularBuffer(buffer, bufferSize);
+    bool success = true;
+    circularBuffer.bytesUntilMessageEnd(&success);
+
+    EXPECT_FALSE(success);
+}
+
+TEST(TMCircularBufferTest, CurrentIndex) {
+    static const uint16_t bufferSize = MAX_MAVLINK_MSG_SIZE;
+    uint8_t buffer[bufferSize];
+    TMCircularBuffer circularBuffer(buffer, bufferSize);
+
+    EXPECT_EQ(circularBuffer.currentIndex(), 0);  // Assuming initial index is 0
+    TMCircularBuffer::MAVLinkByte byte = 0x42;    // Arbitrary byte value
+    circularBuffer.enqueue(byte);
+    circularBuffer.dequeue();
+    EXPECT_EQ(circularBuffer.currentIndex(), 1);
+}
+
+TEST(TMCircularBufferTest, EnqueueDequeueMultipleBytes) {
+    static const uint16_t bufferSize = MAX_MAVLINK_MSG_SIZE;
+    uint8_t buffer[bufferSize];
+    TMCircularBuffer circularBuffer(buffer, bufferSize);
+    TMCircularBuffer::MAVLinkByte bytes[] = {0x01, 0x02, 0x03, 0x04};
+    for (auto byte : bytes) {
+        EXPECT_TRUE(circularBuffer.enqueue(byte));
+    }
+    for (auto byte : bytes) {
+        EXPECT_EQ(circularBuffer.dequeue(), byte);
+    }
+}
+
+TEST(TMCircularBufferTest, EnqueueDequeueFullBuffer) {
+    static const uint16_t bufferSize = MAX_MAVLINK_MSG_SIZE;
+    uint8_t buffer[bufferSize];
+    TMCircularBuffer circularBuffer(buffer, bufferSize);
+
+    for (uint16_t i = 0; i < bufferSize; ++i) {
+        EXPECT_TRUE(circularBuffer.enqueue(static_cast<TMCircularBuffer::MAVLinkByte>(i)));
+    }
+    for (uint16_t i = 0; i < bufferSize; ++i) {
+        EXPECT_EQ(circularBuffer.dequeue(), static_cast<TMCircularBuffer::MAVLinkByte>(i));
+    }
+}
+
+TEST(TMCircularBufferTest, WrapAround) {
+    static const uint16_t bufferSize = MAX_MAVLINK_MSG_SIZE;
+    uint8_t buffer[bufferSize];
+    TMCircularBuffer circularBuffer(buffer, bufferSize);
+    for (uint16_t i = 0; i < bufferSize / 2; ++i) {
+        EXPECT_TRUE(circularBuffer.enqueue(static_cast<TMCircularBuffer::MAVLinkByte>(i)));
+    }
+    for (uint16_t i = 0; i < bufferSize / 2; ++i) {
+        EXPECT_EQ(circularBuffer.dequeue(), static_cast<TMCircularBuffer::MAVLinkByte>(i));
+    }
+    for (uint16_t i = bufferSize / 2; i < bufferSize; ++i) {
+        EXPECT_TRUE(circularBuffer.enqueue(static_cast<TMCircularBuffer::MAVLinkByte>(i)));
+    }
+    for (uint16_t i = bufferSize / 2; i < bufferSize; ++i) {
+        EXPECT_EQ(circularBuffer.dequeue(), static_cast<TMCircularBuffer::MAVLinkByte>(i));
+    }
+}
+
+TEST(TMCircularBufferTest, BytesUntilLastMessageEnd) {
+    static const uint16_t bufferSize = MAX_MAVLINK_MSG_SIZE;
+    uint8_t buffer[bufferSize];
+    TMCircularBuffer circularBuffer(buffer, bufferSize);
+    // Assuming 0xFD marks the end of a message
+    TMCircularBuffer::MAVLinkByte bytes[] = {0x01, 0x02, 0x03, 0x04, 0xFD, 0x05};
+    for (auto byte : bytes) {
+        circularBuffer.enqueue(byte);
+    }
+    EXPECT_EQ(circularBuffer.bytesUntilMessageEnd(), 4);  // The index of the second 0xFD
+}
+
+TEST(TMCircularBufferTest, EnqueueWhenFull) {
+    static const uint16_t bufferSize = MAX_MAVLINK_MSG_SIZE;
+    uint8_t buffer[bufferSize];
+    TMCircularBuffer circularBuffer(buffer, bufferSize);
+
+    for (uint16_t i = 0; i < bufferSize; ++i) {
+        EXPECT_TRUE(circularBuffer.enqueue(static_cast<TMCircularBuffer::MAVLinkByte>(i)));
+    }
+    // Attempt to enqueue when buffer is full
+    EXPECT_FALSE(circularBuffer.enqueue(0xFF));
+}
\ No newline at end of file
diff --git a/TelemetryManager/Tests/temp.txt b/TelemetryManager/Tests/temp.txt
deleted file mode 100644
index e69de29b..00000000
diff --git a/Tools/Firmware/CMakeLists.txt b/Tools/Firmware/CMakeLists.txt
index 770a000e..57afd5a2 100644
--- a/Tools/Firmware/CMakeLists.txt
+++ b/Tools/Firmware/CMakeLists.txt
@@ -22,6 +22,11 @@ include_directories(
 
     ## Telemetry Manager Includes ##
     ${ROOT_DIR}/TelemetryManager/Inc
+    ${ROOT_DIR}/TelemetryManager/Inc/Official_Mavlink_2_Library
+    ${ROOT_DIR}/TelemetryManager/Inc/Official_Mavlink_2_Library/common
+    ${ROOT_DIR}/TelemetryManager/Inc/Official_Mavlink_2_Library/message_definitions
+    ${ROOT_DIR}/TelemetryManager/Inc/Official_Mavlink_2_Library/minimal
+    ${ROOT_DIR}/TelemetryManager/Inc/Official_Mavlink_2_Library/standard
 
     ## Driver Includes ##
     
@@ -84,12 +89,16 @@ set(ATTITUDE_MANAGER_C_SOURCES "${ATTITUDE_MANAGER}/Src/*.c"
                               "${ATTITUDE_MANAGER}/FlightModes/Src/*.c")
 set(ATTITUDE_MANAGER_CXX_SOURCES "${ATTITUDE_MANAGER}/Src/*.cpp"
                                 "${ATTITUDE_MANAGER}/FlightModes/Src/*.cpp")
-set(TELEMETRY_MANAGER_CXX_SOURCES "${TELEMETRY_MANAGER}/Src/*.cpp")
+# set(TELEMETRY_MANAGER_CXX_SOURCES "${TELEMETRY_MANAGER}/Src/*.cpp")
 
 set(SYSTEM_MANAGER ${ROOT_DIR}/SystemManager)
 set(SYSTEM_MANAGER_C_SOURCES "${SYSTEM_MANAGER}/Src/*.c")
 set(SYSTEM_MANAGER_CXX_SOURCES "${SYSTEM_MANAGER}/Src/*.cpp")
 
+set(TELEMETRY_MANAGER ${ROOT_DIR}/TelemetryManager)
+# set(TELEMETRY_MANAGER_C_SOURCES "${TELEMETRY_MANAGER}/Src/*.c")
+set(TELEMETRY_MANAGER_CXX_SOURCES "${TELEMETRY_MANAGER}/Src/*.cpp")
+
 set(DRIVERS_DIR ${ROOT_DIR}/Drivers)
 set(DRIVERS_C_SOURCES "${DRIVERS_DIR}/sensor_fusion/ccontrol/Src/*.c")
 set(DRIVERS_CXX_SOURCES "${DRIVERS_DIR}/sensor_fusion/Src/*.cpp")
diff --git a/Tools/Testing/CMakeLists.txt b/Tools/Testing/CMakeLists.txt
index acb70235..15769215 100644
--- a/Tools/Testing/CMakeLists.txt
+++ b/Tools/Testing/CMakeLists.txt
@@ -73,10 +73,13 @@ set(TM_SRC "${TM_FOLDER}/Src")
 
 include_directories(${TM_INC})
 file(GLOB TM_CODE
-    "${TM_FOLDER}/Tests/generic_test.cpp"
+    "${TM_FOLDER}/Tests/*.cpp"
 
     # add necessary TelemetryManager source files below
-    # "${TM_SRC}/*.cpp"
+    "${TM_SRC}/MavlinkDecoder.cpp"
+    "${TM_SRC}/MavlinkEncoder.cpp"
+    "${TM_SRC}/TMCircularBuffer.cpp"
+    "${TM_SRC}/MavlinkTranslator.cpp"
 )
 
 # Drivers