diff --git a/.gitmodules b/.gitmodules
index 7e0d0de0530ea62291475b0267be349663a37976..9a705d3b07bb818a668f171a3d44d1684f270f40 100755
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,3 @@
-[submodule "mavlink_skyward_lib"]
-	path = mavlink_skyward_lib
-	url = git@git.skywarder.eu:r2a/mavlink_skyward_lib.git
+[submodule "mavlink-skyward-lib"]
+	path = mavlink-skyward-lib
+	url = ../mavlink-skyward-lib
diff --git a/Makefile b/Makefile
index 29cca0ceae155821927de8e086213afef27ee6af..8e221a2e85cb66d11ee5e6320690c7e9a58f18a5 100755
--- a/Makefile
+++ b/Makefile
@@ -4,8 +4,8 @@ LDFLAGS  :=
 BUILD    := ./build
 OBJ_DIR  := $(BUILD)
 TARGET   := mavdecoder
-INCLUDE  := -Imavlink_skyward_lib/
-SRC      := src/mavdecoder.cpp src/decoders.cpp
+INCLUDE  := -Imavlink-skyward-lib/
+SRC      := src/main.cpp
 
 OBJECTS := $(SRC:%.cpp=$(OBJ_DIR)/%.o)
 
diff --git a/Readme.md b/Readme.md
index a488149359d18421413899d7771ccdfbdb8ffd18..91aa8b9028e899dbf8cc81077d1415ad0b7e51eb 100755
--- a/Readme.md
+++ b/Readme.md
@@ -8,34 +8,6 @@ messages (eg. files containing every byte received by the Ground Station over th
 
 1. Compile the sources using `make`
 
-2. Decode the messages using `./mavdecoder <input_file>`
+2. Decode the messages using `./mavdecoder <INPUT>`
 
 The output will be located in the `out/` directory.
-
-### Updating message definitions
-1. Sync the `mavlink_skyward_lib` submodule
-
-2. Generate the new sources:   
-`python3 generate.py <mavlink_lib>`  
-Where `mavlink_lib` is the name of a mavlink message library (the name of one of the folders in `mavlink_skyward_lib/mavlink_lib/`, eg. `hermes`)
-
-3. Copy the contents of `messages_generated.h` into `messages.h`, 
-then modify the `print(...)` functions as needed.  
-**Attention**: Make sure not to overwrite already existing manual edits present in `messages.h`
-
-### Note
-Mavdecoder does not use the build in mavlink parser, but implements its own in 
-order to be able to decode even corrupt messages. Corrupt messages are common when receiving telemetry from a rocket in-flight, and these messages are usually very easy to restore.  
-
-In particular, mavdecoder ignores:
-- CRC verification
-- Message lenght field
-- System & component field
-
-In this way, messages are correctly decoded even if the CRC is wrong and/or the lenght, component id and system id fields are corrupt.  
-This obviosly leads to some false positives, that can, however, be easily detected and discarded (manually).  
-
- 
-
-
-
diff --git a/generate.py b/generate.py
deleted file mode 100755
index 79a3bc1776ca78ceb5988d039a4de006d98cb9c8..0000000000000000000000000000000000000000
--- a/generate.py
+++ /dev/null
@@ -1,219 +0,0 @@
-#!/usr/bin/python3
-# Copyright (c) 2019 Skyward Experimental Rocketry
-# Author: Erbetta Luca
-#
-# 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.
-
-from string import Template
-import os
-import re
-from datetime import datetime
-import argparse
-
-date = datetime.now()
-
-
-def listFiles(dir="."):
-    try:
-        lst = os.listdir(dir)
-    except FileNotFoundError:
-        return []
-
-    files = [os.path.join(dir, x)
-             for x in lst if os.path.isfile(os.path.join(dir, x))]
-    return files
-
-
-def getTelemetryName(filename):
-    m = re.match(r'^mavlink_msg_(?P<tm>[\w_]+).h$', filename)
-    if m:
-        return m.group('tm')
-    else:
-        return None
-
-
-def parseMessages(filename):
-    """Parses a mavlink message from its header. 
-    Specifically, it parses:
-    - message id
-    - message name
-    - message arguments (type and name)
-
-    Arguments:
-        filename -- Mavlink message header
-
-    Returns:
-        List of dictionaries describing the message
-    """
-
-    with open(filename, 'r') as f:
-        mav_source = f.readlines()
-        mav_source = str("".join(mav_source))
-
-        name = getTelemetryName(os.path.basename(filename))  # !type: str
-        name_caps = name.upper()
-
-        rgx_id = r'#define MAVLINK_MSG_ID_{msg_name}\s(?P<id>\d{{1,3}})'. \
-            format(msg_name=name_caps)
-
-        match_id = re.search(rgx_id, mav_source)
-        out = {'name': name}
-
-        if match_id:
-            msg_id = match_id.group('id')
-            out['msg_id'] = msg_id
-        else:
-            return None
-
-        start = mav_source.find("__mavlink_" + name + "_t")
-        end = mav_source.find("mavlink_" + name + "_t;")
-
-        if start < 0 or end < 0:
-            return None
-
-        struct = mav_source[start:end]
-
-        rgx = re.compile(r'(?P<type>[\w\d_]+)\s(?P<name>[\w\d_]+)(\[(?P<mult>\d+)\]){0,1};')
-        has_match = True
-        i = 0
-        out['vars'] = []
-        while has_match:
-            vars_match = rgx.search(struct, i)
-            if vars_match:
-                i = vars_match.end()
-                vartype = vars_match.group('type')
-                varname = vars_match.group('name')
-                #varmult = int(vars_match.groupdict().get('mult', 0))
-                out['vars'] += [(vartype, varname)]
-            else:
-                has_match = False
-
-        return out
-
-    return None
-
-
-def msgDataClassName(message_name: str):
-    title_case = message_name.replace("_", " ").title().replace(" ", "")
-    return "Mav" + title_case + "Data"
-
-
-def isCharType(vartype):
-    """Determines if the provided c/c++ type is a char
-
-    Arguments:
-        vartype -- String with the name of a c/c++ type
-
-    Returns:
-        true if vartype is a char
-    """
-    chartypes = ["uint8_t", "int8_t", "char", "unsigned char"]
-    return vartype in chartypes
-
-
-def genSources(messages):
-    """ Generates decoders.cpp and messages_generated.h sources with all the 
-    mavlink messages
-
-    Arguments:
-        messages -- List of messages & message parameters, obtained with
-                    parseMessages(...)
-    """
-    # decoders.cpp
-    line_template = "\tvec.push_back(\n\t\tnew TelemetryDecoder<{msgclass}" + \
-        ", {mavstruct}>({id}, \"{filename}\"));\n"
-
-    out = ""
-    for msg in messages:
-        classname = msgDataClassName(msg['name'])
-        struct = "mavlink_" + msg['name'] + "_t"
-        msg_id = msg['msg_id']
-        filename = msg['name']
-
-        line = line_template.format(
-            msgclass=classname, mavstruct=struct, id=msg_id, filename=filename)
-
-        out += line
-
-    file_content = ""
-    with(open("src/decoders.cpp.template", 'r')) as f:
-        file_content = "".join(f.readlines())
-
-    file_content = Template(file_content).substitute(
-        decode_vector=out, date=date)
-
-    with(open("src/decoders.cpp", 'w')) as f:
-        f.write(file_content)
-
-    # messages.h
-    with open("src/msg_data_struct.template", "r") as f:
-        class_template = "".join(f.readlines())
-
-    with open("src/messages.h.template", "r") as f:
-        msg_h_template = "".join(f.readlines())
-
-    structs = ""
-    for msg in messages:
-        classname = msgDataClassName(msg['name'])
-        struct = "mavlink_" + msg['name'] + "_t"
-
-        headers = ",".join([x[1] for x in msg['vars']])
-        cast = ["(int)" if isCharType(x[0]) else "" for x in msg['vars']]
-        ostream = " << \",\" << ".join(
-            [c + "mav_pkt." + x[1] for x, c in zip(msg['vars'], cast)])
-
-        struct = Template(class_template).substitute(
-            classname=classname, headers=headers, ostream=ostream,
-            mavstruct=struct)
-        structs += struct
-
-    out = Template(msg_h_template).substitute(
-        msg_data_classes=structs, date=date)
-
-    with open("src/messages_generated.h", 'w') as f:
-        f.write(out)
-
-
-parser = argparse.ArgumentParser(
-    description='Generate c++ sources for decoding raw mavlink messages into'
-    + ' csv files')
-parser.add_argument(
-    'mavlink_lib', help="Mavlink lib to parse the messages from. eg 'hermes'.")
-args = parser.parse_args()
-
-lib_path = "mavlink_skyward_lib/mavlink_lib/" + args.mavlink_lib
-files = listFiles(lib_path)
-
-messages = []
-
-for f in files:
-    b = os.path.basename(f)
-    # if the file contains a mavlink message definition, parse it
-    if b.startswith("mavlink_msg"):
-        msg = parseMessages(f)
-        if msg is None:
-            print("Could not read file: {}".format(b))
-        else:
-            messages += [msg]
-
-if len(messages) > 0:
-    genSources(messages)
-    print("Sources generated successfully.")
-else:
-    print("No messages in " + lib_path)
diff --git a/mavlink-skyward-lib b/mavlink-skyward-lib
new file mode 160000
index 0000000000000000000000000000000000000000..c205269d52a78438fc3adbb2fb247d861fadce17
--- /dev/null
+++ b/mavlink-skyward-lib
@@ -0,0 +1 @@
+Subproject commit c205269d52a78438fc3adbb2fb247d861fadce17
diff --git a/mavlink_skyward_lib b/mavlink_skyward_lib
deleted file mode 160000
index 5a65c1e9046834a5659b69b4a7a5a943001f2b85..0000000000000000000000000000000000000000
--- a/mavlink_skyward_lib
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 5a65c1e9046834a5659b69b4a7a5a943001f2b85
diff --git a/src/decoders.cpp b/src/decoders.cpp
deleted file mode 100755
index b6472c8d1839d6c7e68177635e74c62487594243..0000000000000000000000000000000000000000
--- a/src/decoders.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-/* 
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Erbetta Luca
- *
- * 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.
- */
-
-/*
- ******************************************************************************
- *                  THIS FILE IS AUTOGENERATED. DO NOT EDIT.                  *
- ******************************************************************************
- */
-
-// Generation date: 2019-11-06 01:44:05.542490
-
-
-#include "decoders.h"
-
-void instantiate_decoders(vector<TelemetryDecoderBase*>& vec)
-{
-	vec.push_back(
-		new TelemetryDecoder<MavLoggerTmData, mavlink_logger_tm_t>(162, "logger_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavAckTmData, mavlink_ack_tm_t>(130, "ack_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavNoargTcData, mavlink_noarg_tc_t>(10, "noarg_tc"));
-	vec.push_back(
-		new TelemetryDecoder<MavSysTmData, mavlink_sys_tm_t>(160, "sys_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavAdaTmData, mavlink_ada_tm_t>(167, "ada_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavPingTcData, mavlink_ping_tc_t>(1, "ping_tc"));
-	vec.push_back(
-		new TelemetryDecoder<MavRawEventTcData, mavlink_raw_event_tc_t>(80, "raw_event_tc"));
-	vec.push_back(
-		new TelemetryDecoder<MavFmmTmData, mavlink_fmm_tm_t>(161, "fmm_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavLrTmData, mavlink_lr_tm_t>(181, "lr_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavHrTmData, mavlink_hr_tm_t>(180, "hr_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavCanTmData, mavlink_can_tm_t>(168, "can_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavAdcTmData, mavlink_adc_tm_t>(169, "adc_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavTmtcTmData, mavlink_tmtc_tm_t>(163, "tmtc_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavNackTmData, mavlink_nack_tm_t>(131, "nack_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavAdisTmData, mavlink_adis_tm_t>(170, "adis_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavMpuTmData, mavlink_mpu_tm_t>(171, "mpu_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavTestTmData, mavlink_test_tm_t>(182, "test_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavTelemetryRequestTcData, mavlink_telemetry_request_tc_t>(30, "telemetry_request_tc"));
-	vec.push_back(
-		new TelemetryDecoder<MavDplTmData, mavlink_dpl_tm_t>(166, "dpl_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavIgnTmData, mavlink_ign_tm_t>(165, "ign_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavSmTmData, mavlink_sm_tm_t>(164, "sm_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavUploadSettingTcData, mavlink_upload_setting_tc_t>(21, "upload_setting_tc"));
-	vec.push_back(
-		new TelemetryDecoder<MavGpsTmData, mavlink_gps_tm_t>(172, "gps_tm"));
-	vec.push_back(
-		new TelemetryDecoder<MavStartLaunchTcData, mavlink_start_launch_tc_t>(20, "start_launch_tc"));
-
-}
\ No newline at end of file
diff --git a/src/decoders.cpp.template b/src/decoders.cpp.template
deleted file mode 100755
index e15e32fb2a9465a1a63bb0b960d8a67cd02386a4..0000000000000000000000000000000000000000
--- a/src/decoders.cpp.template
+++ /dev/null
@@ -1,38 +0,0 @@
-/* 
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Erbetta Luca
- *
- * 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.
- */
-
-/*
- ******************************************************************************
- *                  THIS FILE IS AUTOGENERATED. DO NOT EDIT.                  *
- ******************************************************************************
- */
-
-// Generation date: $date
-
-
-#include "decoders.h"
-
-void instantiate_decoders(vector<TelemetryDecoderBase*>& vec)
-{
-$decode_vector
-}
\ No newline at end of file
diff --git a/src/decoders.h b/src/decoders.h
deleted file mode 100755
index 20de1a5e520a39e3ceaedcb82bd09c2ea55e0a9d..0000000000000000000000000000000000000000
--- a/src/decoders.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Erbetta Luca
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#pragma once
-
-#include "messages.h"
-
-#include <fstream>
-#include <iostream>
-#include <string>
-#include <vector>
-#include "../mavlink_skyward_lib/mavlink_lib/hermes/mavlink.h"
-
-using std::ofstream;
-using std::string;
-using std::vector;
-
-class TelemetryDecoderBase
-{
-public:
-    TelemetryDecoderBase(int id, string name)
-    {
-        this->id = id;
-        file.open("out/" + name + ".csv");
-        file.precision(10);
-    }
-
-    virtual ~TelemetryDecoderBase()
-    {
-        file.flush();
-        file.close();
-    }
-
-    virtual void writeHeader()                                   = 0;
-    virtual void decode(MavData packet_data, uint8_t* msg_start) = 0;
-
-    int msgId() { return id; }
-
-protected:
-    ofstream file;
-    int id;
-};
-
-template <class MavDataT, class mav_packet_t>
-class TelemetryDecoder : public TelemetryDecoderBase
-{
-public:
-    TelemetryDecoder(int id, string name) : TelemetryDecoderBase(id, name) {}
-
-    void writeHeader() override
-    {
-        MavData::header(file);
-        file << ",";
-        MavDataT::header(file);
-    }
-
-    void decode(MavData packet_data, uint8_t* msg_start) override
-    {
-        MavDataT data;
-        memcpy(&data.mav_pkt, msg_start, sizeof(mav_packet_t));
-
-        for (unsigned int i = 0; i < MavDataT::messagesPerPacket(); i++)
-        {
-            packet_data.print(file);
-            file << ",";
-
-            data.print(file, i);
-        }
-    }
-};
-
-void instantiate_decoders(vector<TelemetryDecoderBase*>& vec);
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1aad2d2c45482facedd4fe35f7d61e847c7dc935
--- /dev/null
+++ b/src/main.cpp
@@ -0,0 +1,188 @@
+
+// Ignore warnings as these are auto-generated headers made by a third party
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wcast-align"
+#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#include <mavlink_lib/pyxis/mavlink.h>
+#pragma GCC diagnostic pop
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <unordered_map>
+
+const mavlink_message_info_t& get_message_info_for(uint8_t msgid) {
+    static const mavlink_message_info_t infos[256] = MAVLINK_MESSAGE_INFO;
+    return infos[msgid];
+}
+
+class CsvStream {
+public:
+    CsvStream(std::string path, const mavlink_message_info_t& info) :
+        os(), info(info) {
+
+        os.exceptions(std::ifstream::failbit);
+        os.open(path, std::ios::binary);
+        
+        format_message_header();
+    }
+
+    void put(const mavlink_message_t &msg) {
+        format_message(msg);
+    }
+
+private:
+    void format_field(const mavlink_message_t &msg, int id) {
+        const mavlink_field_info_t &field = info.fields[id];
+        if(field.array_length != 0) {
+            os << "<array not implemented>";
+        } else {
+            switch(field.type) {
+            case MAVLINK_TYPE_CHAR:
+                os << _MAV_RETURN_char(&msg, field.wire_offset);
+                break;
+
+            case MAVLINK_TYPE_UINT8_T:
+                os << static_cast<uint64_t>(_MAV_RETURN_uint8_t(&msg, field.wire_offset));
+                break;
+
+            case MAVLINK_TYPE_UINT16_T:
+                os << static_cast<uint64_t>(_MAV_RETURN_uint16_t(&msg, field.wire_offset));
+                break;
+
+            case MAVLINK_TYPE_UINT32_T:
+                os << static_cast<uint64_t>(_MAV_RETURN_uint32_t(&msg, field.wire_offset));
+                break;
+
+            case MAVLINK_TYPE_UINT64_T:
+                os << static_cast<uint64_t>(_MAV_RETURN_uint64_t(&msg, field.wire_offset));
+                break;
+
+            case MAVLINK_TYPE_INT8_T:
+                os << static_cast<int64_t>(_MAV_RETURN_uint8_t(&msg, field.wire_offset));
+                break;
+
+            case MAVLINK_TYPE_INT16_T:
+                os << static_cast<int64_t>(_MAV_RETURN_uint16_t(&msg, field.wire_offset));
+                break;
+
+            case MAVLINK_TYPE_INT32_T:
+                os << static_cast<int64_t>(_MAV_RETURN_uint32_t(&msg, field.wire_offset));
+                break;
+
+            case MAVLINK_TYPE_INT64_T:
+                os << static_cast<int64_t>(_MAV_RETURN_uint64_t(&msg, field.wire_offset));
+                break;
+
+            case MAVLINK_TYPE_FLOAT:
+                os << _MAV_RETURN_float(&msg, field.wire_offset);
+                break;
+
+            case MAVLINK_TYPE_DOUBLE:
+                os << _MAV_RETURN_double(&msg, field.wire_offset);
+                break;
+            }
+        }
+    }
+
+    void format_message(const mavlink_message_t &msg) {
+        bool first = true;
+        for(unsigned i = 0; i < info.num_fields; i++) {
+            if(!first) {
+                os << ",";
+            }
+    
+            format_field(msg, i);
+            first = false;
+        }
+    
+        os << std::endl;
+    }
+
+    void format_message_header() {
+        bool first = true;
+        for(unsigned i = 0; i < info.num_fields; i++) {
+            if(!first) {
+                os << ",";
+            }
+
+            os << std::string(info.fields[i].name);
+            first = false;
+        }
+
+        os << std::endl;
+    }
+
+    std::ofstream os;
+    const mavlink_message_info_t& info;
+};
+
+class Decoder {
+public:
+    Decoder(std::string input, std::string output) :
+        input(input), output(output) {}
+
+    void decode() {
+        std::ifstream is;
+        is.exceptions(std::ifstream::failbit);
+        is.open(input, std::ios::binary);
+
+        std::unordered_map<uint8_t, CsvStream> streams;
+
+        mavlink_message_t msg;
+        mavlink_status_t status;
+        while(is.peek() != EOF) {
+            if(mavlink_parse_char(MAVLINK_COMM_0, is.get(), &msg, &status)) {
+                uint8_t msgid = msg.msgid;
+                const mavlink_message_info_t &info = get_message_info_for(msgid);
+
+                auto it = streams.find(msgid);
+                if(it == streams.end()) {
+                    std::string filename = get_filename_for(info);
+                    it = streams.insert({msgid, CsvStream(filename, info)}).first;
+                }
+
+                it->second.put(msg);
+            }
+        }
+    }
+
+private:
+    std::string get_filename_for(const mavlink_message_info_t &info) {
+        return output + "/" + info.name + ".csv";
+    }
+
+    std::string input;
+    std::string output;
+};
+
+void print_usage() {
+    std::cout <<
+"Skyward mavlink decoder 2.0\n"
+"\n"
+"USAGE:\n"
+"    ./mavdecoder <FILENAME>\n"
+"\n"
+"OPTIONS:\n"
+"    FILENAME     Filename of a mavlink encoded file\n"
+"\n"
+    << std::flush;
+}
+
+int main(int argc, char *argv[]) {
+    if(argc != 2) {
+        print_usage();
+        return -1;
+    }
+
+    try {
+        Decoder decoder(argv[1], "out");
+        decoder.decode();
+    } catch(std::exception &ex) {
+        std::cout << "An exception occurred: " << ex.what() << std::endl;
+        return -1;
+    }
+
+    return 0;
+}
\ No newline at end of file
diff --git a/src/mavdecoder.cpp b/src/mavdecoder.cpp
deleted file mode 100755
index 3945c274e923a67dc0845fdad9053eb70ab9a00b..0000000000000000000000000000000000000000
--- a/src/mavdecoder.cpp
+++ /dev/null
@@ -1,160 +0,0 @@
-/*
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Erbetta Luca
- *
- * 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 <stdio.h>
-#include <stdlib.h>
-#include <sys/stat.h>
-#include <cstdint>
-#include <fstream>
-#include <iostream>
-#include <string>
-#include <vector>
-
-#include "decoders.h"
-
-using namespace std;
-
-static constexpr bool USE_MAVLINK_PARSER = false;
-
-int main(int argc, char** argv)
-{
-    if (argc < 2)
-    {
-        printf("Usage: mavdecoder <file.dat>\n");
-        return 1;
-    }
-    else if (argc > 2)
-    {
-        printf("Too many arguments!\nUsage: mavdecoder <file.dat>\n");
-        return 1;
-    }
-
-    vector<TelemetryDecoderBase*> decoders;
-
-    instantiate_decoders(decoders);
-
-    vector<uint8_t> buf;
-
-    string logfile{argv[1]};
-
-    struct stat buffer;
-    // Check file existance
-    if (stat(logfile.c_str(), &buffer) != 0)
-    {
-        cout << "File " << logfile << " does not exist!\n";
-        return 1;
-    }
-
-    printf(
-        "Decoding %s...\nMake sure there is a directory called 'out' "
-        "in the same folder as this program!\n",
-        logfile.c_str());
-    FILE* f = fopen(logfile.c_str(), "r");
-
-    // Write CSV headers
-    for (TelemetryDecoderBase* td : decoders)
-    {
-        td->writeHeader();
-    }
-
-    // Read binary log
-    do
-    {
-        uint8_t c;
-        int s = fread(&c, 1, 1, f);
-        if (s > 0)
-            buf.push_back(c);
-    } while (!feof(f));
-
-    fclose(f);
-
-    mavlink_message_t msg;
-    mavlink_status_t status;
-
-    unsigned int i = 0;
-
-    while (i < buf.size())
-    {
-        if (USE_MAVLINK_PARSER)
-        {
-            uint8_t parse_result =
-                mavlink_parse_char(MAVLINK_COMM_0, buf.at(i), &msg, &status);
-
-            if (parse_result == 1)
-            {
-                MavData d;
-                d.comp_id = msg.compid;
-                d.len     = msg.len;
-                d.msg_id  = msg.msgid;
-                d.seq     = msg.seq;
-                d.start   = msg.magic;
-                d.sys_id  = msg.sysid;
-
-                for (TelemetryDecoderBase* td : decoders)
-                {
-                    if (td->msgId() == msg.msgid)
-                    {
-                        // uint8_t payload[33*8];
-                        
-                        // memcpy(payload, msg.payload64, 33*8);
-
-                        td->decode(d, (uint8_t*)msg.payload64);
-                    }
-                }
-            }
-        }
-        else
-        {
-            if (buf.at(i) == 0xFE)  // Mavlink start delimiter
-            {
-                MavData d;
-                // Read packet header
-                memcpy(&d, buf.data() + i, sizeof(MavData));
-
-                // bool decoded = false;
-                for (TelemetryDecoderBase* td : decoders)
-                {
-                    if (td->msgId() == d.msg_id)
-                    {
-                        td->decode(d, buf.data() + i + sizeof(MavData));
-                        // decoded = true;
-                    }
-                }
-
-                // if (!decoded)
-                //     printf("Unknown message id (%d) @ %X\n", d.msg_id, i);
-            }
-        }
-        i++;
-    } 
-
-    printf(".\n.\n.\nMessages decoded successfully!\n");
-
-    // Clear things up
-    for (TelemetryDecoderBase* td : decoders)
-    {
-        delete td;
-    }
-
-    return (0);
-}
diff --git a/src/messages.h b/src/messages.h
deleted file mode 100755
index e70fb3d2f01ad986ba213ecbaa8547171301c471..0000000000000000000000000000000000000000
--- a/src/messages.h
+++ /dev/null
@@ -1,693 +0,0 @@
-/* 
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Erbetta Luca
- *
- * 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.
- */
-
-/*
- ******************************************************************************
- *                  THIS FILE IS AUTOGENERATED. DO NOT EDIT.                  *
- *         You should copy the content of this file in messages.h and         *
- *                        perform eventual edits there.                       *
- ******************************************************************************
- */
-
-// Generation date: 2019-11-06 01:40:14.848380
-
-#pragma once
-
-#include "../mavlink_skyward_lib/mavlink_lib/hermes/mavlink.h"
-#include "../mavlink_skyward_lib/bitpacking/hermes/HermesPackets.h"
-
-#include <iostream>
-#include <string>
-
-using std::ostream;
-using std::string;
-
-#pragma pack(push, 1)
-struct MavData
-{
-    uint8_t start;
-    uint8_t len;
-    uint8_t seq;
-    uint8_t sys_id;
-    uint8_t comp_id;
-    uint8_t msg_id;
-
-    static void header(ostream &os)
-    {
-        os << "start,len,seq,sys_id,comp_id,msg_id";
-    }
-
-    void print(ostream &os)
-    {
-        os << (int)start << "," << (int)len << "," << (int)seq << "," 
-           << (int)sys_id << "," << (int)comp_id << "," << (int)msg_id;
-    }
-};
-
-struct MavLoggerTmData
-{
-    mavlink_logger_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,statTooLargeSamples,statDroppedSamples,statQueuedSamples,statBufferFilled,statBufferWritten,statWriteFailed,statWriteError,statWriteTime,statMaxWriteTime,statLogNumber\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.statTooLargeSamples << "," << mav_pkt.statDroppedSamples << "," << mav_pkt.statQueuedSamples << "," << mav_pkt.statBufferFilled << "," << mav_pkt.statBufferWritten << "," << mav_pkt.statWriteFailed << "," << mav_pkt.statWriteError << "," << mav_pkt.statWriteTime << "," << mav_pkt.statMaxWriteTime << "," << mav_pkt.statLogNumber << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavAckTmData
-{
-    mavlink_ack_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "recv_msgid,seq_ack\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.recv_msgid << "," << (int)mav_pkt.seq_ack << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavNoargTcData
-{
-    mavlink_noarg_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "command_id\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.command_id << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavSysTmData
-{
-    mavlink_sys_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,death_stack,logger,ev_broker,pin_obs,fmm,sensor_manager,ada,tmtc,ign,dpl\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << (int)mav_pkt.death_stack << "," << (int)mav_pkt.logger << "," << (int)mav_pkt.ev_broker << "," << (int)mav_pkt.pin_obs << "," << (int)mav_pkt.fmm << "," << (int)mav_pkt.sensor_manager << "," << (int)mav_pkt.ada << "," << (int)mav_pkt.tmtc << "," << (int)mav_pkt.ign << "," << (int)mav_pkt.dpl << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavAdaTmData
-{
-    mavlink_ada_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,target_dpl_altitude,kalman_x0,kalman_x1,kalman_x2,kalman_acc_x0,kalman_acc_x1,kalman_acc_x2,ref_pressure,msl_pressure,ref_pressure_mean,ref_pressure_stddev,ref_pressure_nsamples,ref_altitude,ref_temperature,msl_temperature,state\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.target_dpl_altitude << "," << mav_pkt.kalman_x0 << "," << mav_pkt.kalman_x1 << "," << mav_pkt.kalman_x2 << "," << mav_pkt.kalman_acc_x0 << "," << mav_pkt.kalman_acc_x1 << "," << mav_pkt.kalman_acc_x2 << "," << mav_pkt.ref_pressure << "," << mav_pkt.msl_pressure << "," << mav_pkt.ref_pressure_mean << "," << mav_pkt.ref_pressure_stddev << "," << mav_pkt.ref_pressure_nsamples << "," << mav_pkt.ref_altitude << "," << mav_pkt.ref_temperature << "," << mav_pkt.msl_temperature << "," << (int)mav_pkt.state << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavPingTcData
-{
-    mavlink_ping_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavRawEventTcData
-{
-    mavlink_raw_event_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "Event_id,Topic_id\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.Event_id << "," << (int)mav_pkt.Topic_id << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavFmmTmData
-{
-    mavlink_fmm_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,pin_launch_last_change,pin_nosecone_last_change,state,pin_launch_num_changes,pin_launch_state,pin_nosecone_num_changes,pin_nosecone_state\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.pin_launch_last_change << "," << mav_pkt.pin_nosecone_last_change << "," << (int)mav_pkt.state << "," << (int)mav_pkt.pin_launch_num_changes << "," << (int)mav_pkt.pin_launch_state << "," << (int)mav_pkt.pin_nosecone_num_changes << "," << (int)mav_pkt.pin_nosecone_state << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavLrTmData
-{
-    mavlink_lr_tm_t mav_pkt;
-    LowRateTMPacker p{mav_pkt.payload};
-
-    static void header(ostream &os)
-    {
-        os << "liftoff_ts,liftoff_max_acc_ts,liftoff_max_acc,max_zspeed_ts,max_"
-              "zspeed,max_speed_altitude,apogee_ts,nxp_min_pressure,hw_min_"
-              "pressure,kalman_min_pressure,digital_min_pressure,baro_max_"
-              "altitutde "
-              ",gps_max_altitude,apogee_lat,apogee_lon,drogue_dpl_ts,drogue_"
-              "dpl_max_acc,main_dpl_ts,main_dpl_altitude,main_dpl_zspeed,main_"
-              "dpl_acc\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index;
-
-        long long t;
-        float f;
-
-        p.unpackLiftoffTs(&t, 0);
-        os << t << ",";
-        p.unpackLiftoffMaxAccTs(&t, 0);
-        os << t << ",";
-        p.unpackLiftoffMaxAcc(&f, 0);
-        os << f << ",";
-        p.unpackMaxZspeedTs(&t, 0);
-        os << t << ",";
-        p.unpackMaxZspeed(&f, 0);
-        os << f << ",";
-        p.unpackMaxSpeedAltitude(&f, 0);
-        os << f << ",";
-        p.unpackApogeeTs(&t, 0);
-        os << t << ",";
-        p.unpackNxpMinPressure(&f, 0);
-        os << f << ",";
-        p.unpackHwMinPressure(&f, 0);
-        os << f << ",";
-        p.unpackKalmanMinPressure(&f, 0);
-        os << f << ",";
-        p.unpackDigitalMinPressure(&f, 0);
-        os << f << ",";
-        p.unpackBaroMaxAltitutde(&f, 0);
-        os << f << ",";
-        p.unpackGpsMaxAltitude(&f, 0);
-        os << f << ",";
-        p.unpackApogeeLat(&f, 0);
-        os << f << ",";
-        p.unpackApogeeLon(&f, 0);
-        os << f << ",";
-        p.unpackDrogueDplTs(&t, 0);
-        os << t << ",";
-        p.unpackDrogueDplMaxAcc(&f, 0);
-        os << f << ",";
-        p.unpackMainDplTs(&t, 0);
-        os << t << ",";
-        p.unpackMainDplAltitude(&f, 0);
-        os << f << ",";
-        p.unpackMainDplZspeed(&f, 0);
-        os << f << ",";
-        p.unpackMainDplAcc(&f, 0);
-        os << f << "\n";
-    }
-
-    static unsigned int messagesPerPacket() { return 1; }
-};
-
-struct MavHrTmData
-{
-    mavlink_hr_tm_t mav_pkt;
-    HighRateTMPacker p{mav_pkt.payload};
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,pressure_ada,pressure_digi,msl_altitude,agl_altitude,"
-              "vert_speed,vert_speed_2,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,"
-              "gps_lat,gps_lon,gps_alt,temperature,fmm_state,dpl_state,pin_"
-              "launch,pin_nosecone,gps_fix\n";
-    }
-
-    void print(ostream &os, unsigned int i = 0)
-    {
-        long long t;
-        float f;
-        double d;
-        uint8_t b;
-
-        p.unpackTimestamp(&t, i);
-        os << t << ",";
-        p.unpackPressureAda(&f, i);
-        os << f << ",";
-        p.unpackPressureDigi(&f, i);
-        os << f << ",";
-        p.unpackMslAltitude(&f, i);
-        os << f << ",";
-        p.unpackAglAltitude(&f, i);
-        os << f << ",";
-        p.unpackVertSpeed(&f, i);
-        os << f << ",";
-        p.unpackVertSpeed2(&f, i);
-        os << f << ",";
-        p.unpackAccX(&f, i);
-        os << f << ",";
-        p.unpackAccY(&f, i);
-        os << f << ",";
-        p.unpackAccZ(&f, i);
-        os << f << ",";
-        p.unpackGyroX(&f, i);
-        os << f << ",";
-        p.unpackGyroY(&f, i);
-        os << f << ",";
-        p.unpackGyroZ(&f, i);
-        os << f << ",";
-        p.unpackGpsLat(&d, i);
-        os << (float)d << ",";
-        p.unpackGpsLon(&d, i);
-        os << (float)d << ",";
-        p.unpackGpsAlt(&f, i);
-        os << f << ",";
-        p.unpackTemperature(&f, i);
-        os << f << ",";
-        p.unpackFmmState(&b, i);
-        os << (int)b << ",";
-        p.unpackDplState(&b, i);
-        os << (int)b << ",";
-        p.unpackPinLaunch(&b, i);
-        os << (int)b << ",";
-        p.unpackPinNosecone(&b, i);
-        os << (int)b << ",";
-        p.unpackGpsFix(&b, i);
-        os << (int)b << "\n";
-    }
-
-    static unsigned int messagesPerPacket() { return 4; }
-};
-
-struct MavCanTmData
-{
-    mavlink_can_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "last_sent_ts,last_rcv_ts,n_sent,n_rcv,last_sent,last_rcv\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.last_sent_ts << "," << mav_pkt.last_rcv_ts << "," << mav_pkt.n_sent << "," << mav_pkt.n_rcv << "," << (int)mav_pkt.last_sent << "," << (int)mav_pkt.last_rcv << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavAdcTmData
-{
-    mavlink_adc_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,hw_baro_volt,nxp_baro_volt,hw_baro_pressure,nxp_baro_pressure,battery_voltage,current_sense_1,current_sense_2,hw_baro_flag,nxp_baro_flag\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.hw_baro_volt << "," << mav_pkt.nxp_baro_volt << "," << mav_pkt.hw_baro_pressure << "," << mav_pkt.nxp_baro_pressure << "," << mav_pkt.battery_voltage << "," << mav_pkt.current_sense_1 << "," << mav_pkt.current_sense_2 << "," << (int)mav_pkt.hw_baro_flag << "," << (int)mav_pkt.nxp_baro_flag << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavTmtcTmData
-{
-    mavlink_tmtc_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,parse_state,n_send_queue,max_send_queue,n_send_errors,packet_rx_success_count,packet_rx_drop_count,msg_received,buffer_overrun,parse_error,packet_idx,current_rx_seq,current_tx_seq\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.parse_state << "," << mav_pkt.n_send_queue << "," << mav_pkt.max_send_queue << "," << mav_pkt.n_send_errors << "," << mav_pkt.packet_rx_success_count << "," << mav_pkt.packet_rx_drop_count << "," << (int)mav_pkt.msg_received << "," << (int)mav_pkt.buffer_overrun << "," << (int)mav_pkt.parse_error << "," << (int)mav_pkt.packet_idx << "," << (int)mav_pkt.current_rx_seq << "," << (int)mav_pkt.current_tx_seq << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavNackTmData
-{
-    mavlink_nack_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "recv_msgid,seq_ack\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.recv_msgid << "," << (int)mav_pkt.seq_ack << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavAdisTmData
-{
-    mavlink_adis_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,compass_x,compass_y,compass_z,temp,supply_out,aux_adc\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.acc_x << "," << mav_pkt.acc_y << "," << mav_pkt.acc_z << "," << mav_pkt.gyro_x << "," << mav_pkt.gyro_y << "," << mav_pkt.gyro_z << "," << mav_pkt.compass_x << "," << mav_pkt.compass_y << "," << mav_pkt.compass_z << "," << mav_pkt.temp << "," << mav_pkt.supply_out << "," << mav_pkt.aux_adc << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavMpuTmData
-{
-    mavlink_mpu_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,compass_x,compass_y,compass_z,temp\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.acc_x << "," << mav_pkt.acc_y << "," << mav_pkt.acc_z << "," << mav_pkt.gyro_x << "," << mav_pkt.gyro_y << "," << mav_pkt.gyro_z << "," << mav_pkt.compass_x << "," << mav_pkt.compass_y << "," << mav_pkt.compass_z << "," << mav_pkt.temp << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavTestTmData
-{
-    mavlink_test_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,pressure_hw,temp_analog,temp_imu,battery_volt,th_cut_1,th_cut_2,gps_nsats\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.pressure_hw << "," << mav_pkt.temp_analog << "," << mav_pkt.temp_imu << "," << mav_pkt.battery_volt << "," << mav_pkt.th_cut_1 << "," << mav_pkt.th_cut_2 << "," << (int)mav_pkt.gps_nsats << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavTelemetryRequestTcData
-{
-    mavlink_telemetry_request_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "board_id\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.board_id << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavDplTmData
-{
-    mavlink_dpl_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,cutter_1_test_current,cutter_2_test_current,fsm_state,cutter_state\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.cutter_1_test_current << "," << mav_pkt.cutter_2_test_current << "," << (int)mav_pkt.fsm_state << "," << (int)mav_pkt.cutter_state << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavIgnTmData
-{
-    mavlink_ign_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,n_sent_messages,n_rcv_message,fsm_state,last_event,cmd_bitfield,stm32_bitfield,avr_bitfield\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.n_sent_messages << "," << mav_pkt.n_rcv_message << "," << (int)mav_pkt.fsm_state << "," << (int)mav_pkt.last_event << "," << (int)mav_pkt.cmd_bitfield << "," << (int)mav_pkt.stm32_bitfield << "," << (int)mav_pkt.avr_bitfield << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavSmTmData
-{
-    mavlink_sm_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,task_10hz_min,task_10hz_max,task_10hz_mean,task_10hz_stddev,task_20hz_min,task_20hz_max,task_20hz_mean,task_20hz_stddev,task_250hz_min,task_250hz_max,task_250hz_mean,task_250hz_stddev,sensor_state,state\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.task_10hz_min << "," << mav_pkt.task_10hz_max << "," << mav_pkt.task_10hz_mean << "," << mav_pkt.task_10hz_stddev << "," << mav_pkt.task_20hz_min << "," << mav_pkt.task_20hz_max << "," << mav_pkt.task_20hz_mean << "," << mav_pkt.task_20hz_stddev << "," << mav_pkt.task_250hz_min << "," << mav_pkt.task_250hz_max << "," << mav_pkt.task_250hz_mean << "," << mav_pkt.task_250hz_stddev << "," << mav_pkt.sensor_state << "," << (int)mav_pkt.state << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavUploadSettingTcData
-{
-    mavlink_upload_setting_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "setting,setting_id\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.setting << "," << (int)mav_pkt.setting_id << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavGpsTmData
-{
-    mavlink_gps_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,lat,lon,altitude,vel_north,vel_east,vel_down,vel_mag,n_satellites,fix\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.lat << "," << mav_pkt.lon << "," << mav_pkt.altitude << "," << mav_pkt.vel_north << "," << mav_pkt.vel_east << "," << mav_pkt.vel_down << "," << mav_pkt.vel_mag << "," << mav_pkt.n_satellites << "," << (int)mav_pkt.fix << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavStartLaunchTcData
-{
-    mavlink_start_launch_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "launch_code\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.launch_code << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-
-
-#pragma pack(pop)
-
diff --git a/src/messages.h.template b/src/messages.h.template
deleted file mode 100755
index 7cd50bfe414afa695e118fe00486dc24eb429129..0000000000000000000000000000000000000000
--- a/src/messages.h.template
+++ /dev/null
@@ -1,69 +0,0 @@
-/* 
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Erbetta Luca
- *
- * 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.
- */
-
-/*
- ******************************************************************************
- *                  THIS FILE IS AUTOGENERATED. DO NOT EDIT.                  *
- *         You should copy the content of this file in messages.h and         *
- *                        perform eventual edits there.                       *
- ******************************************************************************
- */
-
-// Generation date: $date
-
-#pragma once
-
-#include "../mavlink_skyward_lib/mavlink_lib/hermes/mavlink.h"
-
-#include <iostream>
-#include <string>
-
-using std::ostream;
-using std::string;
-
-#pragma pack(push, 1)
-struct MavData
-{
-    uint8_t start;
-    uint8_t len;
-    uint8_t seq;
-    uint8_t sys_id;
-    uint8_t comp_id;
-    uint8_t msg_id;
-
-    static void header(ostream &os)
-    {
-        os << "start,len,seq,sys_id,comp_id,msg_id";
-    }
-
-    void print(ostream &os)
-    {
-        os << (int)start << "," << (int)len << "," << (int)seq << "," 
-           << (int)sys_id << "," << (int)comp_id << "," << (int)msg_id;
-    }
-};
-
-$msg_data_classes
-
-#pragma pack(pop)
-
diff --git a/src/messages_generated.h b/src/messages_generated.h
deleted file mode 100755
index 9a7b7a5f71333e40806f9176c287e16be87cc397..0000000000000000000000000000000000000000
--- a/src/messages_generated.h
+++ /dev/null
@@ -1,597 +0,0 @@
-/* 
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Erbetta Luca
- *
- * 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.
- */
-
-/*
- ******************************************************************************
- *                  THIS FILE IS AUTOGENERATED. DO NOT EDIT.                  *
- *         You should copy the content of this file in messages.h and         *
- *                        perform eventual edits there.                       *
- ******************************************************************************
- */
-
-// Generation date: 2019-11-06 01:44:05.542490
-
-#pragma once
-
-#include "../mavlink_skyward_lib/mavlink_lib/hermes/mavlink.h"
-
-#include <iostream>
-#include <string>
-
-using std::ostream;
-using std::string;
-
-#pragma pack(push, 1)
-struct MavData
-{
-    uint8_t start;
-    uint8_t len;
-    uint8_t seq;
-    uint8_t sys_id;
-    uint8_t comp_id;
-    uint8_t msg_id;
-
-    static void header(ostream &os)
-    {
-        os << "start,len,seq,sys_id,comp_id,msg_id";
-    }
-
-    void print(ostream &os)
-    {
-        os << (int)start << "," << (int)len << "," << (int)seq << "," 
-           << (int)sys_id << "," << (int)comp_id << "," << (int)msg_id;
-    }
-};
-
-struct MavLoggerTmData
-{
-    mavlink_logger_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,statTooLargeSamples,statDroppedSamples,statQueuedSamples,statBufferFilled,statBufferWritten,statWriteFailed,statWriteError,statWriteTime,statMaxWriteTime,statLogNumber\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.statTooLargeSamples << "," << mav_pkt.statDroppedSamples << "," << mav_pkt.statQueuedSamples << "," << mav_pkt.statBufferFilled << "," << mav_pkt.statBufferWritten << "," << mav_pkt.statWriteFailed << "," << mav_pkt.statWriteError << "," << mav_pkt.statWriteTime << "," << mav_pkt.statMaxWriteTime << "," << mav_pkt.statLogNumber << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavAckTmData
-{
-    mavlink_ack_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "recv_msgid,seq_ack\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.recv_msgid << "," << (int)mav_pkt.seq_ack << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavNoargTcData
-{
-    mavlink_noarg_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "command_id\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.command_id << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavSysTmData
-{
-    mavlink_sys_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,death_stack,logger,ev_broker,pin_obs,fmm,sensor_manager,ada,tmtc,ign,dpl\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << (int)mav_pkt.death_stack << "," << (int)mav_pkt.logger << "," << (int)mav_pkt.ev_broker << "," << (int)mav_pkt.pin_obs << "," << (int)mav_pkt.fmm << "," << (int)mav_pkt.sensor_manager << "," << (int)mav_pkt.ada << "," << (int)mav_pkt.tmtc << "," << (int)mav_pkt.ign << "," << (int)mav_pkt.dpl << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavAdaTmData
-{
-    mavlink_ada_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,target_dpl_altitude,kalman_x0,kalman_x1,kalman_x2,kalman_acc_x0,kalman_acc_x1,kalman_acc_x2,ref_pressure,msl_pressure,ref_pressure_mean,ref_pressure_stddev,ref_pressure_nsamples,ref_altitude,ref_temperature,msl_temperature,state\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.target_dpl_altitude << "," << mav_pkt.kalman_x0 << "," << mav_pkt.kalman_x1 << "," << mav_pkt.kalman_x2 << "," << mav_pkt.kalman_acc_x0 << "," << mav_pkt.kalman_acc_x1 << "," << mav_pkt.kalman_acc_x2 << "," << mav_pkt.ref_pressure << "," << mav_pkt.msl_pressure << "," << mav_pkt.ref_pressure_mean << "," << mav_pkt.ref_pressure_stddev << "," << mav_pkt.ref_pressure_nsamples << "," << mav_pkt.ref_altitude << "," << mav_pkt.ref_temperature << "," << mav_pkt.msl_temperature << "," << (int)mav_pkt.state << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavPingTcData
-{
-    mavlink_ping_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavRawEventTcData
-{
-    mavlink_raw_event_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "Event_id,Topic_id\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.Event_id << "," << (int)mav_pkt.Topic_id << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavFmmTmData
-{
-    mavlink_fmm_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,pin_launch_last_change,pin_nosecone_last_change,state,pin_launch_num_changes,pin_launch_state,pin_nosecone_num_changes,pin_nosecone_state\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.pin_launch_last_change << "," << mav_pkt.pin_nosecone_last_change << "," << (int)mav_pkt.state << "," << (int)mav_pkt.pin_launch_num_changes << "," << (int)mav_pkt.pin_launch_state << "," << (int)mav_pkt.pin_nosecone_num_changes << "," << (int)mav_pkt.pin_nosecone_state << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavLrTmData
-{
-    mavlink_lr_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "payload\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.payload << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavHrTmData
-{
-    mavlink_hr_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "payload\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.payload << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavCanTmData
-{
-    mavlink_can_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "last_sent_ts,last_rcv_ts,n_sent,n_rcv,last_sent,last_rcv\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.last_sent_ts << "," << mav_pkt.last_rcv_ts << "," << mav_pkt.n_sent << "," << mav_pkt.n_rcv << "," << (int)mav_pkt.last_sent << "," << (int)mav_pkt.last_rcv << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavAdcTmData
-{
-    mavlink_adc_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,hw_baro_volt,nxp_baro_volt,hw_baro_pressure,nxp_baro_pressure,battery_voltage,current_sense_1,current_sense_2,hw_baro_flag,nxp_baro_flag\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.hw_baro_volt << "," << mav_pkt.nxp_baro_volt << "," << mav_pkt.hw_baro_pressure << "," << mav_pkt.nxp_baro_pressure << "," << mav_pkt.battery_voltage << "," << mav_pkt.current_sense_1 << "," << mav_pkt.current_sense_2 << "," << (int)mav_pkt.hw_baro_flag << "," << (int)mav_pkt.nxp_baro_flag << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavTmtcTmData
-{
-    mavlink_tmtc_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,parse_state,n_send_queue,max_send_queue,n_send_errors,packet_rx_success_count,packet_rx_drop_count,msg_received,buffer_overrun,parse_error,packet_idx,current_rx_seq,current_tx_seq\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.parse_state << "," << mav_pkt.n_send_queue << "," << mav_pkt.max_send_queue << "," << mav_pkt.n_send_errors << "," << mav_pkt.packet_rx_success_count << "," << mav_pkt.packet_rx_drop_count << "," << (int)mav_pkt.msg_received << "," << (int)mav_pkt.buffer_overrun << "," << (int)mav_pkt.parse_error << "," << (int)mav_pkt.packet_idx << "," << (int)mav_pkt.current_rx_seq << "," << (int)mav_pkt.current_tx_seq << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavNackTmData
-{
-    mavlink_nack_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "recv_msgid,seq_ack\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.recv_msgid << "," << (int)mav_pkt.seq_ack << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavAdisTmData
-{
-    mavlink_adis_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,compass_x,compass_y,compass_z,temp,supply_out,aux_adc\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.acc_x << "," << mav_pkt.acc_y << "," << mav_pkt.acc_z << "," << mav_pkt.gyro_x << "," << mav_pkt.gyro_y << "," << mav_pkt.gyro_z << "," << mav_pkt.compass_x << "," << mav_pkt.compass_y << "," << mav_pkt.compass_z << "," << mav_pkt.temp << "," << mav_pkt.supply_out << "," << mav_pkt.aux_adc << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavMpuTmData
-{
-    mavlink_mpu_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,compass_x,compass_y,compass_z,temp\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.acc_x << "," << mav_pkt.acc_y << "," << mav_pkt.acc_z << "," << mav_pkt.gyro_x << "," << mav_pkt.gyro_y << "," << mav_pkt.gyro_z << "," << mav_pkt.compass_x << "," << mav_pkt.compass_y << "," << mav_pkt.compass_z << "," << mav_pkt.temp << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavTestTmData
-{
-    mavlink_test_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,pressure_hw,temp_analog,temp_imu,battery_volt,th_cut_1,th_cut_2,gps_nsats\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.pressure_hw << "," << mav_pkt.temp_analog << "," << mav_pkt.temp_imu << "," << mav_pkt.battery_volt << "," << mav_pkt.th_cut_1 << "," << mav_pkt.th_cut_2 << "," << (int)mav_pkt.gps_nsats << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavTelemetryRequestTcData
-{
-    mavlink_telemetry_request_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "board_id\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << (int)mav_pkt.board_id << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavDplTmData
-{
-    mavlink_dpl_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,cutter_1_test_current,cutter_2_test_current,fsm_state,cutter_state\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.cutter_1_test_current << "," << mav_pkt.cutter_2_test_current << "," << (int)mav_pkt.fsm_state << "," << (int)mav_pkt.cutter_state << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavIgnTmData
-{
-    mavlink_ign_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,n_sent_messages,n_rcv_message,fsm_state,last_event,cmd_bitfield,stm32_bitfield,avr_bitfield\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.n_sent_messages << "," << mav_pkt.n_rcv_message << "," << (int)mav_pkt.fsm_state << "," << (int)mav_pkt.last_event << "," << (int)mav_pkt.cmd_bitfield << "," << (int)mav_pkt.stm32_bitfield << "," << (int)mav_pkt.avr_bitfield << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavSmTmData
-{
-    mavlink_sm_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,task_10hz_min,task_10hz_max,task_10hz_mean,task_10hz_stddev,task_20hz_min,task_20hz_max,task_20hz_mean,task_20hz_stddev,task_250hz_min,task_250hz_max,task_250hz_mean,task_250hz_stddev,sensor_state,state\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.task_10hz_min << "," << mav_pkt.task_10hz_max << "," << mav_pkt.task_10hz_mean << "," << mav_pkt.task_10hz_stddev << "," << mav_pkt.task_20hz_min << "," << mav_pkt.task_20hz_max << "," << mav_pkt.task_20hz_mean << "," << mav_pkt.task_20hz_stddev << "," << mav_pkt.task_250hz_min << "," << mav_pkt.task_250hz_max << "," << mav_pkt.task_250hz_mean << "," << mav_pkt.task_250hz_stddev << "," << mav_pkt.sensor_state << "," << (int)mav_pkt.state << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavUploadSettingTcData
-{
-    mavlink_upload_setting_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "setting,setting_id\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.setting << "," << (int)mav_pkt.setting_id << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavGpsTmData
-{
-    mavlink_gps_tm_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "timestamp,lat,lon,altitude,vel_north,vel_east,vel_down,vel_mag,n_satellites,fix\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.timestamp << "," << mav_pkt.lat << "," << mav_pkt.lon << "," << mav_pkt.altitude << "," << mav_pkt.vel_north << "," << mav_pkt.vel_east << "," << mav_pkt.vel_down << "," << mav_pkt.vel_mag << "," << mav_pkt.n_satellites << "," << (int)mav_pkt.fix << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-struct MavStartLaunchTcData
-{
-    mavlink_start_launch_tc_t mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "launch_code\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << mav_pkt.launch_code << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-
-
-
-#pragma pack(pop)
-
diff --git a/src/msg_data_struct.template b/src/msg_data_struct.template
deleted file mode 100755
index df7dc27bcca5728b94ff0d30ed66aa11b3b32abc..0000000000000000000000000000000000000000
--- a/src/msg_data_struct.template
+++ /dev/null
@@ -1,22 +0,0 @@
-struct $classname
-{
-    $mavstruct mav_pkt;
-
-    static void header(ostream &os)
-    {
-        os << "$headers\n";
-    }
-
-    void print(ostream &os, unsigned int index = 0)
-    {
-        (void)index; // Avoid unused warnings
-        
-        os << $ostream << "\n";
-    }
-
-    static unsigned int messagesPerPacket()
-    {
-        return 1;
-    }
-};
-