diff --git a/.gitignore b/.gitignore
index be91b21147e8842750043ac7982987bb86bfb2d4..0c1482597615bca1daaa4fe5261dc01c874d9a1c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -11,9 +11,9 @@ bin/*
 .*.sw?
 .*.pyc
 
-.project 
-.cproject 
-.settings 
+.project
+.cproject
+.settings
 STM32F429zi_Discovery.xml
 
 ._*
@@ -25,5 +25,5 @@ build
 .vscode/*
 
 store.json
-scripts/homeone/event_header_generator/generated/
+*/generated/
 core
diff --git a/sbs.conf b/sbs.conf
index cd6dccc8ea50a1d6603845d59454bfc3c1516cac..26cfb6ac6e53bc86336095d6a2a6241073aa696b 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -475,6 +475,7 @@ Include:    %shared %spi
 Defines:
 Main:       drivers/test-l3gd20
 
+<<<<<<< HEAD
 [test-lsm9ds1]
 Type:       test
 BoardId:    stm32f407vg_stm32f4discovery
@@ -490,6 +491,15 @@ BinName:    test-lsm9ds1-class
 Include:    %shared %spi
 Defines:    -DDEBUG
 Main:       drivers/test-lsm9ds1-class
+=======
+[test-l3gd20-fifo]
+Type:       test
+BoardId:    stm32f429zi_stm32f4discovery
+BinName:    test-l3gd20-fifo
+Include:    %shared %spi
+Defines:
+Main:       drivers/test-l3gd20-fifo
+>>>>>>> origin/testing
 
 [test-rls]
 Type:       test
diff --git a/scripts/eventgen/EventFunctions.cpp.template b/scripts/eventgen/EventFunctions.cpp.template
new file mode 100644
index 0000000000000000000000000000000000000000..1611abafb11180cdadeafae5294f45acc4f85f78
--- /dev/null
+++ b/scripts/eventgen/EventFunctions.cpp.template
@@ -0,0 +1,53 @@
+/* Copyright (c) 2018-2020 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Alvise de' Faveri Tron
+ *
+ * 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.                  *
+ ******************************************************************************
+ */
+
+// Autogen date:    {date}
+
+
+#include "Events.h"
+#include "Topics.h"
+
+#include <map>
+
+string getEventString(uint8_t event)
+{{
+    static const map<uint8_t, string> event_string_map {{
+{event_map_data}
+    }};
+    auto   it  = event_string_map.find(event);
+    return it == event_string_map.end() ? "EV_UNKNOWN" : it->second;
+}}
+
+string getTopicString(uint8_t topic)
+{{
+	static const map<uint8_t, string> topic_string_map{{
+{topic_map_data}
+	}};
+	auto it = topic_string_map.find(topic);
+	return it == topic_string_map.end() ? "TOPIC_UNKNOWN" : it->second;
+}}
\ No newline at end of file
diff --git a/scripts/eventgen/Events.h.template b/scripts/eventgen/Events.h.template
new file mode 100644
index 0000000000000000000000000000000000000000..bd9cc25367ec63690cfc588a877778f110f09791
--- /dev/null
+++ b/scripts/eventgen/Events.h.template
@@ -0,0 +1,57 @@
+/* Copyright (c) 2018-2020 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Alvise de' Faveri Tron
+ *
+ * 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.                  *
+ ******************************************************************************
+ */
+
+// Autogen date:    {date}
+
+#pragma once
+
+#include <cstdint>
+#include <string>
+
+#include "events/Event.h"
+#include "events/EventBroker.h"
+#include "EventClasses.h"
+#include "Topics.h"
+
+using std::string;
+using std::map;
+
+/**
+ * Definition of all events signals.
+enum Events : uint8_t
+{{
+{enum_data}
+}};
+
+/**
+ * @brief Returns the name of the provided event
+ *
+ * @param event
+ * @return string
+ */
+string getEventString(uint8_t event);
\ No newline at end of file
diff --git a/scripts/eventgen/README.md b/scripts/eventgen/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..7bfb1c9b0625ffdfc86794475246b92e6251e586
--- /dev/null
+++ b/scripts/eventgen/README.md
@@ -0,0 +1,10 @@
+# Events Generator Script
+
+This script generates Events.h, Topics.h and EventFunctions.cpp from a list of
+SCXML files representing the state machines of a project.
+
+To execute the script:
+
+`./eventgen.py <LIST_OF_SCXML_FILES>`
+
+The files will be generated in the generated/ folder.
\ No newline at end of file
diff --git a/scripts/eventgen/Topics.h.template b/scripts/eventgen/Topics.h.template
new file mode 100644
index 0000000000000000000000000000000000000000..2ce2c2c536a6ba099b7c72ab2df987ccf9449fd3
--- /dev/null
+++ b/scripts/eventgen/Topics.h.template
@@ -0,0 +1,54 @@
+/* Copyright (c) 2018-2020 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Alvise de' Faveri Tron
+ *
+ * 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.                  *
+ ******************************************************************************
+ */
+
+// Autogen date:    {date}
+
+#pragma once
+
+#include <stdint.h>
+#include <string>
+
+using std::map;
+using std::string;
+
+/**
+ * Definition of various event topics to use in the EventBroker
+ */
+enum Topics : uint8_t
+{{
+{enum_data}
+}};
+
+/**
+ * @brief Returns the name of the provided event
+ *
+ * @param event
+ * @return string
+ */
+string getTopicString(uint8_t topic);
+
diff --git a/scripts/eventgen/eventgen.py b/scripts/eventgen/eventgen.py
new file mode 100755
index 0000000000000000000000000000000000000000..9c36223a3266a22a65961776074efcfc0bd14656
--- /dev/null
+++ b/scripts/eventgen/eventgen.py
@@ -0,0 +1,200 @@
+#!/usr/bin/python3
+
+# Copyright (c) 2018-2020 Skyward Experimental Rocketry
+# Authors: Luca Erbetta, Alvise de' Faveri Tron
+#
+# 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.
+#
+import re
+import datetime
+from os.path import join
+import os
+
+import xml.etree.ElementTree as ET
+import sys
+from collections import OrderedDict
+
+OUTPUT_FOLDER = "generated"
+
+#
+# ASCII art banner
+#
+def printBanner():
+    print("+--------------------------------------------------------+")
+    print("|     ____  _                                _           |")
+    print("|    / ___|| | ___   ___      ____ _ _ __ __| |          |")
+    print("|    \\___ \\| |/ / | | \\ \\ /\\ / / _` | '__/ _` |          |")
+    print("|     ___) |   <| |_| |\\ V  V / (_| | | | (_| |          |")
+    print("|    |____/|_|\\_\\\\__, | \\_/\\_/ \\__,_|_|  \\__,_|          |")
+    print("|           _____|___/            _                      |")
+    print("|          | ____|_   _____ _ __ | |_ __ _  ___ _ __     |")
+    print("|          |  _| \\ \\ / / _ \\ '_ \\| __/ _` |/ _ \\ '_ \\    |")
+    print("|          | |___ \\ V /  __/ | | | || (_| |  __/ | | |   |")
+    print("|          |_____| \\_/ \\___|_| |_|\\__\\__, |\\___|_| |_|   |")
+    print("|                                    |___/               |")
+    print("+--------------------------------------------------------+")
+
+#
+# Parse SCXML file(s)
+#
+def parse_scxml(files):
+    events = []
+    topics = []
+
+    for file in files:
+        print("parsing " + str(file))
+        tree = ET.parse(str(file))
+        root = tree.getroot()
+
+        # find all transitions and related events
+        first = 0
+        for child in root:
+            for tran in child.iter('{http://www.w3.org/2005/07/scxml}transition'):
+                if(tran.attrib['target'] != 'final'):
+                    ev = tran.attrib['event']
+
+                    try:
+                        topics.append("TOPIC_" + ev.split('.')[0])
+                        events.append(ev.split('.')[1])
+
+                    except:
+                        print("Cannot parse the event name... Forgot the topic?")
+                        print(tran.tag, tran.attrib)
+
+    # remove duplicates
+    events = list(OrderedDict.fromkeys(events))
+    topics = list(OrderedDict.fromkeys(topics))
+    events.sort()
+    topics.sort()
+
+    print("{} events loaded.".format(len(events)))
+    print("{} topics loaded.".format(len(topics)))
+
+    return events, topics
+
+#
+# Generate Events.h
+#
+def generate_events(events, date):
+    enum_str = ""
+    event_map_str = ""
+
+    # generate string
+    for e in events:
+        endl = ",\n" if e != events[-1] else ""
+        enum_str += "    " + e + \
+            (" = EV_FIRST_SIGNAL" if e == events[0] else "") + endl
+        event_map_str += "        {{ {event}, {string} }}{endl}".format(
+            event=e, string='"' + e + '"', endl=endl)
+
+    # read template
+    with open('Events.h.template', 'r') as template_file:
+        template = template_file.read()
+
+    # write template
+    template = template.format(date=date, enum_data=enum_str)
+
+    with open(join(OUTPUT_FOLDER, 'Events.h'), 'w') as header_file:
+        header_file.write(template)
+
+    return event_map_str
+
+#
+# Generate Topics.h
+#
+def generate_topics(topics, date):
+    enum_str = ""
+    topic_map_str = ""
+
+    for t in topics:
+        endl = ",\n" if t != topics[-1] else ""
+        enum_str += "    " + t + endl
+        topic_map_str += "        {{ {topics}, {string} }}{endl}".format(
+            topics=t, string='"' + t + '"', endl=endl)
+
+    with open('Topics.h.template', 'r') as template_file:
+        template = template_file.read()
+
+    template = template.format(date=date, enum_data=enum_str)
+
+    with open(join(OUTPUT_FOLDER, 'Topics.h'), 'w') as header_file:
+        header_file.write(template)
+
+    return topic_map_str
+
+#
+# Generate EventFunctions.cpp
+#
+def generate_functions(event_map_str, topic_map_str, date):
+    with open('EventFunctions.cpp.template', 'r') as cpp_template_file:
+        cpp = cpp_template_file.read()
+
+    cpp = cpp.format(date=date, event_map_data=event_map_str, topic_map_data=topic_map_str)
+
+    with open(join(OUTPUT_FOLDER, 'EventFunctions.cpp'), 'w') as cpp_file:
+        cpp_file.write(cpp)
+
+#
+# Main
+#
+printBanner()
+print()
+print("Skyward SCXML event generator")
+print()
+
+if(len(sys.argv) < 2):
+    print("Error: no SCXML files provided.\nUsage: eventgen.py <SCXML_FILES>")
+    sys.exit(-1)
+
+if not os.path.exists(OUTPUT_FOLDER):
+    os.mkdir(OUTPUT_FOLDER)
+
+date = datetime.datetime.now()
+
+print("-"*30)
+print("Parsing SCXML files...")
+events, topics = parse_scxml(sys.argv[1:])
+
+print("-"*30)
+print("Generating Events.h...")
+event_map_str = generate_events(events, date)
+print("OK\n")
+
+print("-"*30)
+print("Generating Topics.h...")
+topic_map_str = generate_topics(topics, date)
+print("OK\n")
+
+print("-"*30)
+print("Generating EventFunctions.cpp...")
+generate_functions(event_map_str, topic_map_str, date)
+print("OK\n")
+
+
+# print
+print("-"*30)
+print("Events:\n")
+for e in events:
+    print(e)
+print("\n\nTopics:\n")
+for t in topics:
+    print(t)
+
+print("-"*30)
+print("... Done.")
diff --git a/scripts/homeone/mavlink_client/Makefile b/scripts/mavlink_client/Makefile
similarity index 100%
rename from scripts/homeone/mavlink_client/Makefile
rename to scripts/mavlink_client/Makefile
diff --git a/scripts/mavlink_client/README.md b/scripts/mavlink_client/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..4a3c225d75fc187df6b0ac5550cb8bad7f174c9b
--- /dev/null
+++ b/scripts/mavlink_client/README.md
@@ -0,0 +1,14 @@
+# Mavlink Clinet
+
+These scripts can be used to transmit and receive mavlink data from a PC, either
+connected via serial port to a transceiver or directly connected to one of the boards.
+
+`make` compiles the 3 `.c` files:
+
+* continuous_tx: sends a message of raw bytes with a given length at a certain
+frequency. This was used to test the LoRa modules.
+* mavlink_demo_tx: sends a message whenever it receives a char on stdin
+* mavlink_demo_rx: receives and prints packets, eventually sending back ana ACK.
+
+`plot.py`: reads value on stdin and plots them. Can be used piping in the output
+of mavlink_demo_rx
\ No newline at end of file
diff --git a/scripts/homeone/mavlink_client/continuous_tx.c b/scripts/mavlink_client/continuous_tx.c
similarity index 100%
rename from scripts/homeone/mavlink_client/continuous_tx.c
rename to scripts/mavlink_client/continuous_tx.c
diff --git a/scripts/homeone/mavlink_client/mavlink_demo_rx.c b/scripts/mavlink_client/mavlink_demo_rx.c
similarity index 98%
rename from scripts/homeone/mavlink_client/mavlink_demo_rx.c
rename to scripts/mavlink_client/mavlink_demo_rx.c
index e33145dfb2ba3eb00163ba5c0329326d3e5bdfc5..95c49aa44d440abf09822dd0889435438775b51b 100644
--- a/scripts/homeone/mavlink_client/mavlink_demo_rx.c
+++ b/scripts/mavlink_client/mavlink_demo_rx.c
@@ -1,6 +1,6 @@
 
 /**************************************************
-Ssimple demo that receives mavlink
+Simple demo that receives mavlink
 messages on the serial port and prints them on the screen.
 
 **************************************************/
diff --git a/scripts/homeone/mavlink_client/mavlink_demo_tx.c b/scripts/mavlink_client/mavlink_demo_tx.c
similarity index 100%
rename from scripts/homeone/mavlink_client/mavlink_demo_tx.c
rename to scripts/mavlink_client/mavlink_demo_tx.c
diff --git a/scripts/homeone/mavlink_client/plot.py b/scripts/mavlink_client/plot.py
similarity index 100%
rename from scripts/homeone/mavlink_client/plot.py
rename to scripts/mavlink_client/plot.py
diff --git a/scripts/homeone/mavlink_client/rs232.c b/scripts/mavlink_client/rs232.c
similarity index 100%
rename from scripts/homeone/mavlink_client/rs232.c
rename to scripts/mavlink_client/rs232.c
diff --git a/scripts/homeone/mavlink_client/rs232.h b/scripts/mavlink_client/rs232.h
similarity index 100%
rename from scripts/homeone/mavlink_client/rs232.h
rename to scripts/mavlink_client/rs232.h
diff --git a/scripts/anakin.sh b/scripts/old/anakin.sh
similarity index 100%
rename from scripts/anakin.sh
rename to scripts/old/anakin.sh
diff --git a/scripts/anakin_client_curses/.gitignore b/scripts/old/anakin_client_curses/.gitignore
similarity index 100%
rename from scripts/anakin_client_curses/.gitignore
rename to scripts/old/anakin_client_curses/.gitignore
diff --git a/scripts/anakin_client_curses/Makefile b/scripts/old/anakin_client_curses/Makefile
similarity index 100%
rename from scripts/anakin_client_curses/Makefile
rename to scripts/old/anakin_client_curses/Makefile
diff --git a/scripts/anakin_client_curses/client.c b/scripts/old/anakin_client_curses/client.c
similarity index 100%
rename from scripts/anakin_client_curses/client.c
rename to scripts/old/anakin_client_curses/client.c
diff --git a/scripts/homeone/event_header_generator/EventFunctions.cpp.template b/scripts/old/excel_eventgen/EventFunctions.cpp.template
similarity index 100%
rename from scripts/homeone/event_header_generator/EventFunctions.cpp.template
rename to scripts/old/excel_eventgen/EventFunctions.cpp.template
diff --git a/scripts/homeone/event_header_generator/Events.h.template b/scripts/old/excel_eventgen/Events.h.template
similarity index 100%
rename from scripts/homeone/event_header_generator/Events.h.template
rename to scripts/old/excel_eventgen/Events.h.template
diff --git a/scripts/homeone/event_header_generator/README.md b/scripts/old/excel_eventgen/README.md
similarity index 100%
rename from scripts/homeone/event_header_generator/README.md
rename to scripts/old/excel_eventgen/README.md
diff --git a/scripts/homeone/event_header_generator/Topics.h.template b/scripts/old/excel_eventgen/Topics.h.template
similarity index 100%
rename from scripts/homeone/event_header_generator/Topics.h.template
rename to scripts/old/excel_eventgen/Topics.h.template
diff --git a/scripts/homeone/event_header_generator/credentials.json b/scripts/old/excel_eventgen/credentials.json
similarity index 100%
rename from scripts/homeone/event_header_generator/credentials.json
rename to scripts/old/excel_eventgen/credentials.json
diff --git a/scripts/homeone/event_header_generator/event_generator.py b/scripts/old/excel_eventgen/event_generator.py
similarity index 100%
rename from scripts/homeone/event_header_generator/event_generator.py
rename to scripts/old/excel_eventgen/event_generator.py
diff --git a/scripts/gen_fault_headers.py b/scripts/old/gen_fault_headers.py
similarity index 100%
rename from scripts/gen_fault_headers.py
rename to scripts/old/gen_fault_headers.py
diff --git a/src/shared/drivers/BusTemplate.h b/src/shared/drivers/BusTemplate.h
index 0652a840bf017fa009fbc6883e85947ac5454318..0a9ebfbc6b2fab8322900d69f2bd86067938703e 100644
--- a/src/shared/drivers/BusTemplate.h
+++ b/src/shared/drivers/BusTemplate.h
@@ -110,6 +110,9 @@ private:
 
     inline void _write(uint8_t byte) const
     {
+        // Wait until the peripheral is ready to transmit
+        while ((getSPIAddr(N)->SR & SPI_SR_TXE) == 0)
+            ;
         getSPIAddr(N)->DR = byte;
         while ((getSPIAddr(N)->SR & SPI_SR_RXNE) == 0)
             ;
@@ -127,6 +130,9 @@ private:
 
     inline uint8_t _read() const
     {
+        // Wait until the peripheral is ready to transmit
+        while ((getSPIAddr(N)->SR & SPI_SR_TXE) == 0)
+            ;
         getSPIAddr(N)->DR = 0;
         while ((getSPIAddr(N)->SR & SPI_SR_RXNE) == 0)
             ;
@@ -157,6 +163,9 @@ private:
      */
     inline uint8_t _transfer(uint8_t* byte)
     {
+        // Wait until the peripheral is ready to transmit
+        while ((getSPIAddr(N)->SR & SPI_SR_TXE) == 0)
+            ;
         getSPIAddr(N)->DR = *byte;
         while ((getSPIAddr(N)->SR & SPI_SR_RXNE) == 0)
             ;
diff --git a/src/shared/drivers/HardwareTimer.h b/src/shared/drivers/HardwareTimer.h
index cb82f77b63815471b8cd60cb673be7ec53b65afa..267014839b9ace07595246a649e8eb1a38daa2ec 100644
--- a/src/shared/drivers/HardwareTimer.h
+++ b/src/shared/drivers/HardwareTimer.h
@@ -40,22 +40,22 @@ public:
      * @brief returns the timer clock frequency before the prescaler.
      * Function borrowed from the SyncronizedServo class in Miosix.
      *
-     * @return unsigned int Prescaler input frequency
+     * @return Prescaler input frequency
      */
-    static unsigned int getPrescalerInputFrequency(InputClock input_clock)
+    static uint32_t getPrescalerInputFrequency(InputClock input_clock)
     {
         // The global variable SystemCoreClock from ARM's CMSIS allows to
         // know
         // the CPU frequency.
-        unsigned int freq = SystemCoreClock;
+        uint32_t freq = SystemCoreClock;
 
 // The position of the PPRE1 bit in RCC->CFGR is different in some stm32
 #ifdef _ARCH_CORTEXM3_STM32
-        const unsigned int ppre1 = 8;
+        const uint32_t ppre1 = 8;
 #error "Architecture not currently supported"
 #else  // stm32f2 and f4
-        const unsigned int ppre1 = 10;
-        const unsigned int ppre2 = 13;
+        const uint32_t ppre1 = 10;
+        const uint32_t ppre2 = 13;
 #endif
         // The timer frequency may however be a submultiple of the CPU
         // frequency,
@@ -104,7 +104,7 @@ public:
      * @param    psc_input_freq Input frequency of the timer's prescaler, see
      *                          TimerUtils::getPrescalerInputFrequency()
      */
-    HardwareTimer(TIM_TypeDef* timer, unsigned int psc_input_freq)
+    HardwareTimer(TIM_TypeDef* timer, uint32_t psc_input_freq)
         : tim(timer), prescaler_freq(psc_input_freq)
     {
     }
@@ -137,6 +137,8 @@ public:
      */
     float toMicroSeconds(Type ticks);
 
+    uint64_t toIntMicroSeconds(Type ticks);
+
     /**
      * @brief Converts from ticks to milliseconds, using the current prescaler
      * setting
@@ -185,9 +187,9 @@ public:
 
 private:
     TIM_TypeDef* tim;
-    unsigned prescaler_freq;
+    uint32_t prescaler_freq;
 
-    bool ticking = false;
+    bool ticking       = false;
     uint16_t prescaler = 0;
     Type auto_reload =
         static_cast<Type>(-1);  // Max value of Type (Type is unsigned)
@@ -253,6 +255,13 @@ float HardwareTimer<Type>::toMicroSeconds(Type ticks)
     return (1.0f * ticks * 1000000 * (1 + prescaler)) / prescaler_freq;
 }
 
+template <typename Type>
+uint64_t HardwareTimer<Type>::toIntMicroSeconds(Type ticks)
+{
+    return ((uint64_t)ticks * 1000000 * (uint64_t)(1 + prescaler)) /
+           (uint64_t)prescaler_freq;
+}
+
 template <typename Type>
 float HardwareTimer<Type>::toMilliSeconds(Type ticks)
 {
diff --git a/src/shared/drivers/Xbee/Xbee.h b/src/shared/drivers/Xbee/Xbee.h
index d269b1e27052a71a807ac8571cc865dd792cad03..ea5625585ead196ab292591194e8f5e2942fcf47 100644
--- a/src/shared/drivers/Xbee/Xbee.h
+++ b/src/shared/drivers/Xbee/Xbee.h
@@ -119,7 +119,7 @@ public:
          unsigned int send_timeout = 1000)
         : send_timeout(send_timeout), spi_xbee(bus, cs), attn(attn), rst(rst)
     {
-        spi_xbee.config.br = SPIBaudRate::DIV_128;
+        spi_xbee.config.clock_div = SPIClockDivider::DIV128;
 
         // No need to configure before each transaction, we are the only device
         // on the bus.
diff --git a/src/shared/drivers/spi/SPIBus.cpp b/src/shared/drivers/spi/SPIBus.cpp
index c7dc7c803a4972a0150f1b9ac6d916f63ec6af67..afe76d97724c74860d7ed6cafb4423ed24d07707 100644
--- a/src/shared/drivers/spi/SPIBus.cpp
+++ b/src/shared/drivers/spi/SPIBus.cpp
@@ -34,17 +34,22 @@ void SPIBus::configure(SPIBusConfig new_config)
         first_config_applied = true;
         config               = new_config;
 
-        // Clean CR1
+        // Wait until the peripheral is done before changing configuration
+        while ((spi->SR & SPI_SR_TXE) == 0)
+            ;
+        while ((spi->SR & SPI_SR_BSY) == 1)
+            ;
+        
         spi->CR1 = 0;
 
-        // Configure clock division (BR bits)
-        spi->CR1 |= (static_cast<uint16_t>(config.br) & 0x0007) << 3;
         // Configure CPOL & CPHA bits
-        spi->CR1 |= ((uint16_t)config.cpol & 0x0001) << 1 |
-                    ((uint16_t)config.cpha & 0x0001);
+        spi->CR1 |= static_cast<uint32_t>(config.mode);
+
+        // Configure clock division (BR bits)
+        spi->CR1 |= static_cast<uint32_t>(config.clock_div);
 
         // Configure LSBFIRST bit
-        spi->CR1 |= (uint16_t)config.lsb_first << 7;
+        spi->CR1 |= static_cast<uint32_t>(config.bit_order);
 
         spi->CR1 |= SPI_CR1_SSI | SPI_CR1_SSM  // Use software chip-select
                     | SPI_CR1_MSTR             // Master mode
diff --git a/src/shared/drivers/spi/SPIInterface.h b/src/shared/drivers/spi/SPIInterface.h
index e7d9955e9a5c516e250aa65fdfcd9be740b8fd29..7a3eaa1fd3497e99dc517492833fe82d69badd81 100644
--- a/src/shared/drivers/spi/SPIInterface.h
+++ b/src/shared/drivers/spi/SPIInterface.h
@@ -32,24 +32,48 @@ using miosix::delayUs;
 using miosix::GpioPin;
 
 /**
- * @brief SPI baud rate selection parameter.
- * SPI clock frequency will be equal to the SPI peripheral bus clock speed
- * divided by the value specified in this enum.
+ * @brief SPI Clock divider.
+ * SPI clock frequency will be equal to the SPI peripheral bus clock speed (see
+ * datasheet) divided by the value specified in this enum.
  *
  * Eg: DIV_2 --> spi clock freq = f_PCLK / 2
  *
- * See SPI->CR1 on the datasheet for further information.
+ * See register CR1  of the SPI peripheral on the reference manual for further
+ * information.
  */
-enum class SPIBaudRate
+enum class SPIClockDivider : uint8_t
 {
-    DIV_2   = 0,
-    DIV_4   = 1,
-    DIV_8   = 2,
-    DIV_16  = 3,
-    DIV_32  = 4,
-    DIV_64  = 5,
-    DIV_128 = 6,
-    DIV_256 = 7,
+    DIV2   = 0x00,
+    DIV4   = 0x08,
+    DIV8   = 0x10,
+    DIV16  = 0x18,
+    DIV32  = 0x20,
+    DIV64  = 0x28,
+    DIV128 = 0x30,
+    DIV256 = 0x38,
+};
+
+/**
+ * @brief SPI Mode.
+ * See slave device datasheet for information on which one to use.
+ */
+enum class SPIMode : uint8_t
+{
+    MODE0 = 0,  ///> CPOL = 0, CPHA = 0
+    MODE1 = 1,  ///> CPOL = 0, CPHA = 1
+    MODE2 = 2,  ///> CPOL = 1, CPHA = 0
+    MODE3 = 3   ///> CPOL = 1, CPHA = 1
+};
+
+/**
+ * @brief SPI Bit Order
+ * See register CR1  of the SPI peripheral on the reference manual for further
+ * information.
+ */
+enum class SPIBitOrder : uint8_t
+{
+    MSB_FIRST = 0,
+    LSB_FIRST = 0x80
 };
 
 /**
@@ -58,10 +82,10 @@ enum class SPIBaudRate
  */
 struct SPIBusConfig
 {
-    SPIBaudRate br = SPIBaudRate::DIV_2;  ///> Peripheral clock division
-    uint8_t cpol   = 0;                   ///> Clock polarity (0 - 1)
-    uint8_t cpha   = 0;                   ///> Clock phase (0 - 1)
-    bool lsb_first = false;               ///> MSB or LSB first
+    SPIClockDivider clock_div =
+        SPIClockDivider::DIV2;               ///> Peripheral clock division
+    SPIMode mode          = SPIMode::MODE0;  ///> Clock polarity (0 - 1)
+    SPIBitOrder bit_order = SPIBitOrder::MSB_FIRST;  ///> MSB or LSB first
 
     unsigned int cs_setup_time_us = 0;  ///> How long to wait before starting a
                                         ///> a trasmission after CS is set (us)
@@ -73,10 +97,9 @@ struct SPIBusConfig
     {
         // Compare member-by-member
         // clang-format off
-        return  br == other.br 
-             && cpol == other.cpol 
-             && cpha == other.cpha 
-             && lsb_first == other.lsb_first 
+        return  clock_div == other.clock_div 
+             && mode == other.mode
+             && bit_order == other.bit_order
              && cs_setup_time_us == other.cs_setup_time_us 
              && cs_setup_time_us == other.cs_hold_time_us;
         // clang-format on
diff --git a/src/shared/drivers/spi/SPITransaction.cpp b/src/shared/drivers/spi/SPITransaction.cpp
index 8e5746907547e82f25cd8deeaf7f3e9b17a16ca4..a6f026c6b821325aa8e4e836cbd8fbdb924aa4b3 100644
--- a/src/shared/drivers/spi/SPITransaction.cpp
+++ b/src/shared/drivers/spi/SPITransaction.cpp
@@ -77,12 +77,11 @@ uint8_t SPITransaction::read(uint8_t reg, bool set_read_bit)
     if (set_read_bit)
         reg = reg | 0x80;
 
-    uint8_t out;
     bus.select(cs);
     bus.write(&reg, 1);
-    bus.read(&out, 1);
+    bus.read(&reg, 1);
     bus.deselect(cs);
-    return out;
+    return reg;
 }
 
 void SPITransaction::read(uint8_t reg, uint8_t* data, size_t size,
diff --git a/src/shared/sensors/L3GD20.h b/src/shared/sensors/L3GD20.h
index 9133d5b5090e3fd47ba9257ef8c295c0f5afd1f3..69c60af52f763be70850af28aceb376d2bad9046 100644
--- a/src/shared/sensors/L3GD20.h
+++ b/src/shared/sensors/L3GD20.h
@@ -1,3 +1,4 @@
+
 /**
  * Copyright (c) 2019 Skyward Experimental Rocketry
  * Authors: Luca Erbetta
@@ -32,6 +33,12 @@
 using miosix::GpioPin;
 using std::array;
 
+struct L3GD20Data
+{
+    uint64_t timestamp;
+    Vec3 gyro;
+};
+
 class L3GD20 : public GyroSensor
 {
 public:
@@ -44,11 +51,12 @@ public:
 
     enum class OutPutDataRate
     {
-        ODR_95  = 0x00,
-        ODR_190 = 0x01,
-        ODR_380 = 0x02,
-        ODR_760 = 0x03
+        ODR_95  = 95,
+        ODR_190 = 190,
+        ODR_380 = 380,
+        ODR_760 = 760
     };
+
     /**
      * @brief Creates an instance of an L3GD20 sensor
      *
@@ -57,21 +65,57 @@ public:
      * @param    range Full Scale Range (See datasheet)
      * @param    odr Output Data Rate (See datasheet)
      * @param    cutoff_freq Low pass filter cutoff frequency (See datasheet)
-     * @param    fifo_enabled Fifo enabled
-     * @param    fifo_watermark FIFO watermark level in range [1,32] (used for
-     * interrupt generation, see datasheet).
      */
     L3GD20(SPIBusInterface& bus, GpioPin cs,
            FullScaleRange range = FullScaleRange::FS_250,
            OutPutDataRate odr   = OutPutDataRate::ODR_95,
-           uint8_t cutoff_freq = 0x03, bool fifo_enabled = false,
-           unsigned int fifo_watermark = 24)
-        : fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
-          spislave(bus, cs), fs(range), odr(odr), cutoff_freq(cutoff_freq)
+           uint8_t cutoff_freq  = 0x03)
+        : L3GD20(bus, cs, {}, range, odr, cutoff_freq)
     {
         // Configure SPI
-        spislave.config.br = SPIBaudRate::DIV_64;
-        // memset(last_fifo, 0, sizeof(Vec3) * 32);
+        spislave.config.clock_div = SPIClockDivider::DIV32;
+    }
+
+    /**
+     * @brief Creates an instance of an L3GD20 sensor
+     *
+     * @param    bus SPI bus the sensor is connected to
+     * @param    cs Chip Select GPIO
+     * @param    cfg Custom SPI bus configuration
+     * @param    range Full Scale Range (See datasheet)
+     * @param    odr Output Data Rate (See datasheet)
+     * @param    cutoff_freq Low pass filter cutoff selector (See datasheet)
+     */
+    L3GD20(SPIBusInterface& bus, GpioPin cs, SPIBusConfig cfg,
+           FullScaleRange range = FullScaleRange::FS_250,
+           OutPutDataRate odr   = OutPutDataRate::ODR_95,
+           uint8_t cutoff_freq  = 0x03)
+        : spislave(bus, cs, cfg), fs(range), odr(odr), cutoff_freq(cutoff_freq)
+    {
+        switch (fs)
+        {
+            case FullScaleRange::FS_250:
+                sensitivity = SENSITIVITY_250;
+                break;
+            case FullScaleRange::FS_500:
+                sensitivity = SENSITIVITY_500;
+                break;
+            case FullScaleRange::FS_2000:
+                sensitivity = SENSITIVITY_2000;
+                break;
+        }
+    }
+
+    /**
+     * @brief Enables storing samples in a FIFO, must be called before init().
+     *
+     * @param fifo_watermark How many samples in the FIFO when the fifo
+     * watermark input is generated on INT2.
+     */
+    void enableFifo(unsigned int fifo_watermark)
+    {
+        fifo_enabled         = true;
+        this->fifo_watermark = fifo_watermark;
     }
 
     bool init()
@@ -87,8 +131,6 @@ public:
             return false;
         }
 
-        // uint8_t ctrl4 = spi.read(REG_CTRL4);
-
         switch (fs)
         {
             case FullScaleRange::FS_250:
@@ -103,6 +145,7 @@ public:
             default:
                 break;
         }
+
         if (fifo_enabled)
         {
             // Enable fifo
@@ -111,23 +154,42 @@ public:
             // Set watermark level to fifo_watermark samples
             uint8_t fifo_ctrl = fifo_watermark;
 
-            // Set fifo to FIFO mode
+            // Set fifo to STREAM mode
             fifo_ctrl |= 0x02 << 5;
 
             spi.write(REG_FIFO_CTRL, fifo_ctrl);
 
-            // Enable FIFO watermark interrupt
+            // Enable FIFO watermark interrupt on INT2
             spi.write(REG_CTRL3, 0x04);
         }
+        else
+        {
+            // Enable DRDY interrupt on INT2
+            spi.write(REG_CTRL3, 0x08);
+        }
+
         // Enter normal mode, enable output
         uint8_t ctrl1 = 0x0F;
 
-        // Configure ODR
-        ctrl1 |= static_cast<uint8_t>(odr) << 6;
-
         // Configure cutoff frequency
         ctrl1 |= (cutoff_freq & 0x03) << 4;
 
+        // Configure ODR
+        switch (odr)
+        {
+            case OutPutDataRate::ODR_95:
+                break;
+            case OutPutDataRate::ODR_190:
+                ctrl1 |= 1 << 6;
+                break;
+            case OutPutDataRate::ODR_380:
+                ctrl1 |= 2 << 6;
+                break;
+            case OutPutDataRate::ODR_760:
+                ctrl1 |= 3 << 6;
+                break;
+        }
+
         spi.write(REG_CTRL1, ctrl1);
 
         return true;
@@ -137,75 +199,123 @@ public:
 
     bool onSimpleUpdate()
     {
-        float scale = static_cast<int>(fs);
-
-        if (!fifo_enabled)
+        if (!fifo_enabled)  // FIFO not enabled
         {
-            uint8_t data[6];
             // Read output data registers (X, Y, Z)
             {
                 SPITransaction spi(spislave);
-                spi.read(REG_OUT_X_L | 0x40, data, 6);
+                spi.read(REG_OUT_X_L | 0x40, buf, 6);
             }
 
-            int16_t x = data[0] | data[1] << 8;
-            int16_t y = data[2] | data[3] << 8;
-            int16_t z = data[4] | data[5] << 8;
+            int16_t x = buf[0] | buf[1] << 8;
+            int16_t y = buf[2] | buf[3] << 8;
+            int16_t z = buf[4] | buf[5] << 8;
             // printf("%02X,%02X,%02X\n", x, y, z);
 
-            mLastGyro =
-                Vec3(x * scale / 65535, y * scale / 65535, z * scale / 65535);
+            last_fifo[0] = {interrupt_us, toRadiansPerSecond(x, y, z)};
         }
+
         else  // FIFO is enabled
         {
-            uint8_t buf[192];
-
             SPITransaction spi(spislave);
             // Read last fifo level
-            uint8_t fifo_src = spi.read(REG_FIFO_SRC);
-            uint8_t ovr      = (fifo_src & 0x40);
-            last_fifo_level  = fifo_src & 0x1F;
-
-            // if ovr --> fifo is full --> level = level + 1
-            if (ovr)
-            {
-                ++last_fifo_level;
-            }
+            uint8_t fifo_src   = spi.read(REG_FIFO_SRC);
+            uint8_t ovr        = (fifo_src & 0x40) >> 7;  // Overrun bit
+            uint8_t fifo_level = (fifo_src & 0x1F) + ovr;
 
             // Read fifo
-            spi.read(REG_OUT_X_L | 0x40, buf, last_fifo_level * 6);
-
-            // Convert units & store the FIFO content
-            for (uint8_t i = 0; i < last_fifo_level; i++)
-            {
-                int16_t x = buf[i * 6] | buf[i * 6 + 1] << 8;
-                int16_t y = buf[i * 6 + 2] | buf[i * 6 + 3] << 8;
-                int16_t z = buf[i * 6 + 4] | buf[i * 6 + 5] << 8;
+            spi.read(REG_OUT_X_L | 0x40, buf, fifo_level * 6);
 
-                Vec3 t = Vec3(x * scale / 65535, y * scale / 65535,
-                              z * scale / 65535);
+            uint64_t dt = dt_interrupt / last_fifo_level;
 
-                last_fifo[i] = t;
+            uint8_t duplicates = 0;
+            for (uint8_t i = 0; i < fifo_level; ++i)
+            {
+                bool rmv;
+                // Check for duplicates: there seems to be a bug where the
+                // sensor occasionaly shifts out the same sample two times in a
+                // row: discard one
+                if (i < fifo_level - 1)
+                {
+                    rmv = true;
+                    for (uint8_t j = 0; j < 6; ++j)
+                    {
+                        if (buf[i * 6 + j] != buf[(i + 1) * 6 + j])
+                        {
+                            rmv = false;
+                            break;
+                        }
+                    }
+                }
+                else
+                {
+                    rmv = false;
+                }
+
+                if (rmv)
+                {
+                    // Skip this sample;
+                    ++duplicates;
+                    continue;
+                }
+
+                // Assign a timestamp to each sample in the FIFO
+                // Samples before the watermark are older, after the
+                // watermark are younger.
+                last_fifo[i - duplicates].timestamp =
+                    interrupt_us +
+                    ((int)i - (int)fifo_watermark - (int)duplicates) * dt;
+                last_fifo[i - duplicates].gyro =
+                    toRadiansPerSecond(buf[i * 6] | buf[i * 6 + 1] << 8,
+                                       buf[i * 6 + 2] | buf[i * 6 + 3] << 8,
+                                       buf[i * 6 + 4] | buf[i * 6 + 5] << 8);
             }
+
+            last_fifo_level = fifo_level - duplicates;
         }
 
         return true;
     }
 
-    const array<Vec3, 32>& getLastFifo() const { return last_fifo; }
+    L3GD20Data getLastSample() { return last_fifo[last_fifo_level - 1]; }
+
+    void IRQupdateTimestamp(uint64_t ts)
+    {
+        dt_interrupt = ts - interrupt_us;
+        interrupt_us = ts;
+    }
+
+    const array<L3GD20Data, 32>& getLastFifo() const { return last_fifo; }
     uint8_t getLastFifoSize() const { return last_fifo_level; }
 
 private:
-    bool fifo_enabled = false;
-    unsigned int fifo_watermark;
+    Vec3 toRadiansPerSecond(int16_t x, int16_t y, int16_t z)
+    {
+        return Vec3(x * sensitivity, y * sensitivity, z * sensitivity);
+    }
 
-    array<Vec3, 32> last_fifo;
-    uint8_t last_fifo_level = 0;
+    static constexpr float SENSITIVITY_250  = 0.00875f;
+    static constexpr float SENSITIVITY_500  = 0.0175f;
+    static constexpr float SENSITIVITY_2000 = 0.070f;
 
     SPISlave spislave;
-    FullScaleRange fs;
-    OutPutDataRate odr;
-    uint8_t cutoff_freq;
+
+    FullScaleRange fs   = FullScaleRange::FS_250;
+    OutPutDataRate odr  = OutPutDataRate::ODR_760;
+    uint8_t cutoff_freq = 0x03;
+
+    bool fifo_enabled           = false;
+    unsigned int fifo_watermark = 24;
+
+    float sensitivity = SENSITIVITY_250;
+
+    uint64_t interrupt_us = 0;
+    uint64_t dt_interrupt = 0;
+
+    array<L3GD20Data, 32> last_fifo;
+    uint8_t last_fifo_level = 1;
+
+    uint8_t buf[192];
 
     constexpr static uint8_t WHO_AM_I_VAL = 212;
 
diff --git a/src/shared/sensors/MS580301BA07/MS580301BA07.h b/src/shared/sensors/MS580301BA07/MS580301BA07.h
index 5131e32496e1b7e79068ffd5118a72957949daab..5f8a02d55e805dd9510cc9f9f529834f7154ba20 100644
--- a/src/shared/sensors/MS580301BA07/MS580301BA07.h
+++ b/src/shared/sensors/MS580301BA07/MS580301BA07.h
@@ -37,7 +37,7 @@ public:
     MS580301BA07(SPIBusInterface& bus, GpioPin cs)
         : MS580301BA07(bus, cs, SPIBusConfig{})
     {
-        spi_ms5803.config.br = SPIBaudRate::DIV_128;
+        spi_ms5803.config.clock_div = SPIClockDivider::DIV128;
     }
 
     MS580301BA07(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config)
diff --git a/src/tests/catch/spidriver/FakeSPIBus.h b/src/tests/catch/spidriver/FakeSPIBus.h
index 2a395552b1fe4d9698a7854154b3d543a270b623..94087b088fa4957db9d30b9520a4f3edc7374dde 100644
--- a/src/tests/catch/spidriver/FakeSPIBus.h
+++ b/src/tests/catch/spidriver/FakeSPIBus.h
@@ -151,14 +151,14 @@ void FakeSPIBus::configure(SPIBusConfig new_config)
         // Clean CR1
         spi->CR1 = 0;
 
-        // Configure clock division (BR bits)
-        spi->CR1 |= (static_cast<uint16_t>(config.br) & 0x0007) << 3;
         // Configure CPOL & CPHA bits
-        spi->CR1 |= ((uint16_t)config.cpol & 0x0001) << 1 |
-                    ((uint16_t)config.cpha & 0x0001);
+        spi->CR1 |= static_cast<uint32_t>(config.mode);
 
+        // Configure clock division (BR bits)
+        spi->CR1 |= static_cast<uint32_t>(config.clock_div);
+        
         // Configure LSBFIRST bit
-        spi->CR1 |= (uint16_t)config.lsb_first << 7;
+        spi->CR1 |= static_cast<uint32_t>(config.bit_order);
 
         spi->CR1 |= SPI_CR1_SSI | SPI_CR1_SSM  // Use software chip-select
                     | SPI_CR1_MSTR             // Master mode
diff --git a/src/tests/catch/spidriver/test-spidriver.cpp b/src/tests/catch/spidriver/test-spidriver.cpp
index 5438d812fa9bbff845e426a997a30dfe08c9e993..8b2f47f7edea4fedcaeab4c58f4cd9874d97a343 100644
--- a/src/tests/catch/spidriver/test-spidriver.cpp
+++ b/src/tests/catch/spidriver/test-spidriver.cpp
@@ -56,80 +56,102 @@ TEST_CASE("SPIBus - Bus Configuration")
     SECTION("Configure & check CR1")
     {
         SPIBusConfig config;
-        config.br = SPIBaudRate::DIV_16;
-
-        config.cpha      = 1;
-        config.cpol      = 1;
-        config.lsb_first = true;
-
-        uint32_t expected_CR1 = 0x03DF;
-
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
-
-        // Change config
-        config.br = SPIBaudRate::DIV_256;
-
-        config.cpha      = 0;
-        config.cpol      = 0;
-        config.lsb_first = false;
-
-        expected_CR1 = 0x037C;
-
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
-
-        config.cpha      = 0;
-        config.cpol      = 1;
-        config.lsb_first = false;
-
-        expected_CR1 = 0x037E;
-
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
+        REQUIRE(spi.CR1 == 0);
 
-        config.cpha      = 1;
-        config.cpol      = 0;
-        config.lsb_first = false;
+        SECTION("Mode")
+        {
+            config.mode = SPIMode::MODE0;
+            uint32_t expected_CR1 = 0x0344;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.mode = SPIMode::MODE1;
+            expected_CR1 = 0x0345;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.mode = SPIMode::MODE2;
+            expected_CR1 = 0x0346;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.mode = SPIMode::MODE3;
+            expected_CR1 = 0x0347;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
 
-        expected_CR1 = 0x037D;
+        }
 
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
+        SECTION("Clock Divider")
+        {
+            config.clock_div = SPIClockDivider::DIV2;
+            uint32_t expected_CR1 = 0x0344;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV4;
+            expected_CR1 = 0x034C;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV8;
+            expected_CR1 = 0x0354;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV16;
+            expected_CR1 = 0x035C;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV32;
+            expected_CR1 = 0x0364;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+            
+            config.clock_div = SPIClockDivider::DIV64;
+            expected_CR1 = 0x036C;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV128;
+            expected_CR1 = 0x0374;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.clock_div = SPIClockDivider::DIV256;
+            expected_CR1 = 0x037C;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+        }
 
-        // Reapply same config
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
+        SECTION("Bit order")
+        {
+            config.bit_order = SPIBitOrder::MSB_FIRST;
+            uint32_t expected_CR1 = 0x0344;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+
+            config.bit_order = SPIBitOrder::LSB_FIRST;
+            expected_CR1 = 0x03C4;   
+            bus.configure(config);
+            REQUIRE(spi.CR1 == expected_CR1);
+        }
+      
     }
 
     SECTION("Disable configuration")
     {
         SPIBusConfig config;
-        config.br = SPIBaudRate::DIV_16;
+        config.clock_div = SPIClockDivider::DIV16;
 
-        config.cpha      = 1;
-        config.cpol      = 1;
-        config.lsb_first = true;
+        config.mode      = SPIMode::MODE3;
+        config.bit_order = SPIBitOrder::LSB_FIRST;
 
         bus.enableSlaveConfiguration(false);
         bus.configure(config);
         REQUIRE(spi.CR1 == 0);
     }
-
-    SECTION("Wrong parameters")
-    {
-        SPIBusConfig config;
-        config.br = SPIBaudRate::DIV_16;
-
-        config.cpha      = 8;
-        config.cpol      = 9;
-        config.lsb_first = true;
-
-        uint32_t expected_CR1 = 0x03DE;
-
-        bus.configure(config);
-        REQUIRE(spi.CR1 == expected_CR1);
-    }
 }
 
 TEST_CASE("SPIBus - Chip select")
@@ -157,11 +179,10 @@ TEST_CASE("SPIBus - One byte operations")
     FakeSPIBus bus{&spi};
 
     SPIBusConfig config;
-    config.br = SPIBaudRate::DIV_16;
+    config.clock_div = SPIClockDivider::DIV16;
 
-    config.cpha      = 1;
-    config.cpol      = 1;
-    config.lsb_first = true;
+    config.mode      = SPIMode::MODE3;
+    config.bit_order = SPIBitOrder::LSB_FIRST;
 
     bus.configure(config);
     bus.select(spi.cs);
@@ -209,11 +230,10 @@ TEST_CASE("SPIBus - Multi byte operations")
     FakeSPIBus bus{&spi};
 
     SPIBusConfig config;
-    config.br = SPIBaudRate::DIV_16;
+    config.clock_div = SPIClockDivider::DIV16;
 
-    config.cpha      = 1;
-    config.cpol      = 1;
-    config.lsb_first = true;
+    config.mode      = SPIMode::MODE3;
+    config.bit_order = SPIBitOrder::LSB_FIRST;
 
     bus.configure(config);
     bus.select(spi.cs);
@@ -264,8 +284,8 @@ TEST_CASE("SPITransaction - writes")
     MockSPIBus bus{};
     SPIBusConfig config1{};
 
-    config1.cpha = 1;
-    config1.br   = SPIBaudRate::DIV_32;
+    config1.mode = SPIMode::MODE1;
+    config1.clock_div  = SPIClockDivider::DIV32;
 
     bus.expected_config = config1;
 
@@ -360,8 +380,8 @@ TEST_CASE("SPITransaction - reads")
 
     SPIBusConfig config1;
 
-    config1.cpha = 1;
-    config1.br   = SPIBaudRate::DIV_32;
+    config1.mode = SPIMode::MODE1;
+    config1.clock_div  = SPIClockDivider::DIV32;
 
     bus.expected_config = config1;
 
@@ -450,8 +470,8 @@ TEST_CASE("SPITransaction - transfer")
 
     SPIBusConfig config1;
 
-    config1.cpha = 1;
-    config1.br   = SPIBaudRate::DIV_32;
+    config1.mode = SPIMode::MODE1;
+    config1.clock_div  = SPIClockDivider::DIV32;
 
     bus.expected_config = config1;
 
@@ -460,7 +480,7 @@ TEST_CASE("SPITransaction - transfer")
         SPISlave slave(bus, GpioPin(GPIOA_BASE, 1), config1);
         SPITransaction spi(slave);
 
-        uint8_t buf[4]  = {4, 3, 2, 1};
+        uint8_t buf[4] = {4, 3, 2, 1};
         uint8_t cmp[4] = {4, 3, 2, 1};
 
         spi.transfer(buf, 0);
diff --git a/src/tests/drivers/test-l3gd20-fifo.cpp b/src/tests/drivers/test-l3gd20-fifo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..31beec225602bd435c864e1183b02f73946d1cc2
--- /dev/null
+++ b/src/tests/drivers/test-l3gd20-fifo.cpp
@@ -0,0 +1,246 @@
+/**
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta
+ *
+ * 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 <array>
+
+#include "diagnostic/CpuMeter.h"
+#include "drivers/HardwareTimer.h"
+#include "drivers/spi/SPIDriver.h"
+#include "sensors/L3GD20.h"
+
+using namespace miosix;
+using std::array;
+
+typedef Gpio<GPIOF_BASE, 7> GpioSck;
+typedef Gpio<GPIOF_BASE, 8> GpioMiso;
+typedef Gpio<GPIOF_BASE, 9> GpioMosi;
+
+typedef Gpio<GPIOA_BASE, 2> GpioINT2;
+
+static constexpr unsigned int FIFO_WATERMARK = 12;
+
+// Expected frequency from the datasheet is 760 Hz, but due to clock
+// misalignment / temperature errors and other factors, the observed clock (and
+// output data rate) is the following:
+static constexpr float SAMPLE_FREQUENCY = 782.3f;
+static constexpr int NUM_SAMPLES        = SAMPLE_FREQUENCY * 120;
+
+// FD
+void enableInterrupt();
+void configure();
+
+struct GyroSample
+{
+    int fifo_num;
+    L3GD20Data gyro;
+    int level;
+    uint64_t wtm_delta;
+    float cpu;
+    uint64_t update;
+};
+
+// GyroSample data[NUM_SAMPLES];
+GyroSample data[NUM_SAMPLES];
+int data_counter = 0;
+
+// High resolution hardware timer using TIM5
+HardwareTimer<uint32_t> hrclock{
+    TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)};
+
+// Last interrupt received timer tick
+volatile uint32_t last_watermark_tick;  // Stores the high-res tick of the last
+                                        // interrupt (L3GD20 watermark event)
+volatile uint32_t watermark_delta;  // Tick delta between the last 2 watermark
+                                    // events
+
+// L3GD20 SPI
+SPIBus bus(SPI5);
+GpioPin cs(GPIOC_BASE, 1);
+
+L3GD20* gyro = nullptr;
+
+// Interrupt handlers
+void __attribute__((naked)) EXTI2_IRQHandler()
+{
+    saveContext();
+    asm volatile("bl _Z20EXTI2_IRQHandlerImplv");
+    restoreContext();
+}
+
+void __attribute__((used)) EXTI2_IRQHandlerImpl()
+{
+    // Current high resolution tick
+    uint32_t tick       = hrclock.tick();
+    watermark_delta     = tick - last_watermark_tick;
+    last_watermark_tick = tick;
+    // Pass tick microseconds to sensor
+    if (gyro != nullptr)
+    {
+        gyro->IRQupdateTimestamp(hrclock.toIntMicroSeconds(tick));
+    }
+
+    EXTI->PR |= EXTI_PR_PR2;  // Reset pending register
+}
+
+int main()
+{
+    Thread::sleep(5000);
+
+    configure();
+
+    // Setup sensor
+    gyro = new L3GD20(bus, cs, L3GD20::FullScaleRange::FS_250,
+                      L3GD20::OutPutDataRate::ODR_760, 0x03);
+
+    gyro->enableFifo(FIFO_WATERMARK);
+
+    // Init
+    while (!gyro->init())
+    {
+    }
+
+    // Sample NUM_SAMPLES data
+    int fifo_num = 0;
+    while (data_counter < NUM_SAMPLES)
+    {
+        long last_tick = miosix::getTick();
+
+        // Read the fifo
+        uint32_t update = hrclock.tick();
+        gyro->onSimpleUpdate();
+        update = hrclock.tick() - update;
+
+        uint8_t level =
+            gyro->getLastFifoSize();  // Number of samples in the FIFO
+
+        // Get the FIFO
+        const array<L3GD20Data, 32>& fifo = gyro->getLastFifo();
+
+        // Store everything in the data buffer
+        for (int i = 0; i < level; i++)
+        {
+            // data[data_counter++] = fifo[i];
+            data[data_counter++] = {fifo_num,
+                                    fifo[i],
+                                    level,
+                                    hrclock.toIntMicroSeconds(watermark_delta),
+                                    averageCpuUtilization(),
+                                    hrclock.toIntMicroSeconds(update)};
+
+            // Stop if we have enough data
+            if (data_counter >= NUM_SAMPLES)
+            {
+                break;
+            }
+        }
+        ++fifo_num;
+
+        // Wait until fifo has about 25 samples
+        Thread::sleepUntil(last_tick + 25.5 * 1000 / SAMPLE_FREQUENCY);
+    }
+
+    // Dump buffer content as CSV on the serial (might take a while)
+    // printf("t,x,y,z\n");
+    printf("FIFO_num,timestamp,int_delta,read_time,sample_delta,x,y,z,cpu\n");
+
+    for (int i = 1; i < data_counter; i++)
+    {
+        // clang-format off
+        printf("%d,%llu,%llu,%llu,%llu,%f,%f,%f,%.2f\n", 
+                data[i].fifo_num,
+                data[i].gyro.timestamp, 
+                data[i].wtm_delta,
+                data[i].update,
+                (data[i].gyro.timestamp - data[i - 1].gyro.timestamp),
+                data[i].gyro.gyro.getX(), 
+                data[i].gyro.gyro.getY(), 
+                data[i].gyro.gyro.getZ(),
+                data[i].cpu);
+        // clang-format on
+    }
+
+    printf("\n\n\nend.\n");
+    for (;;)
+    {
+        Thread::sleep(1000);
+    }
+}
+
+void configure()
+{
+    {
+        FastInterruptDisableLock dLock;
+
+        RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
+        RCC->APB2ENR |= RCC_APB2ENR_SPI5EN;
+
+        GpioSck::mode(Mode::ALTERNATE);
+        GpioMiso::mode(Mode::ALTERNATE);
+        GpioMosi::mode(Mode::ALTERNATE);
+
+        GpioSck::alternateFunction(5);
+        GpioMiso::alternateFunction(5);
+        GpioMosi::alternateFunction(5);
+
+        // Interrupt
+        GpioINT2::mode(Mode::INPUT_PULL_DOWN);
+
+        cs.mode(Mode::OUTPUT);
+    }
+
+    cs.high();
+
+    enableInterrupt();
+
+    // High resolution clock configuration
+    hrclock.setPrescaler(382);
+    hrclock.start();
+}
+
+void enableInterrupt()
+{
+    {
+        FastInterruptDisableLock l;
+        RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
+    }
+    // Refer to the datasheet for a detailed description on the procedure and
+    // interrupt registers
+
+    // Clear the mask on the wanted line
+    EXTI->IMR |= EXTI_IMR_MR2;
+
+    // Trigger the interrupt on a falling edge
+    // EXTI->FTSR |= EXTI_FTSR_TR2;
+
+    // Trigger the interrupt on a rising edge
+    EXTI->RTSR |= EXTI_RTSR_TR2;
+
+    EXTI->PR |= EXTI_PR_PR2;  // Reset pending register
+
+    // Enable interrupt on PA2 in SYSCFG
+    SYSCFG->EXTICR[0] &= 0xFFFFF0FF;
+
+    // // Enable the interrput in the interrupt controller
+    NVIC_EnableIRQ(EXTI2_IRQn);
+    NVIC_SetPriority(EXTI2_IRQn, 15);
+}
\ No newline at end of file
diff --git a/src/tests/drivers/test-l3gd20.cpp b/src/tests/drivers/test-l3gd20.cpp
index f5a63718ccb9427fc4f07f0d0f18afe5fc78ddd0..227dd44f16fc632840792cb352717cbc636b42fd 100644
--- a/src/tests/drivers/test-l3gd20.cpp
+++ b/src/tests/drivers/test-l3gd20.cpp
@@ -37,28 +37,20 @@ typedef Gpio<GPIOF_BASE, 9> GpioMosi;
 
 typedef Gpio<GPIOA_BASE, 2> GpioINT2;
 
-static constexpr bool FIFO_ENABLED           = true;
-static constexpr unsigned int FIFO_WATERMARK = 24;
-// Expected frequency from the datasheet is 760 Hz, but due to clock
-// misalignment / temperature errors and other factors, the observed clock (and
-// output data rate) is the following:
-static constexpr float SAMPLE_FREQUENCY = 782.3f;
-static constexpr int NUM_SAMPLES        = 10000;
+static constexpr int NUM_SAMPLES = 500 * 5;  // 120 seconds of data
 
 void enableInterrupt();
+void configure();
 
 struct GyroSample
 {
-    int fifo_num;
-    float timestamp;
+    uint64_t timestamp;
+    uint64_t sample_delta;
     Vec3 data;
-    int level;
-    float wtm_delta;
     float cpu;
 };
 
-// 364 KB buffer to store up to 30 seconds of data @ 760 Hz
-GyroSample data[22800];
+GyroSample data[NUM_SAMPLES];
 int data_counter = 0;
 
 // High resolution hardware timer using TIM5
@@ -66,16 +58,16 @@ HardwareTimer<uint32_t> hrclock{
     TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)};
 
 // Last interrupt received timer tick
-volatile uint32_t last_watermark_tick;  // Stores the high-res tick of the last
-                                        // interrupt (L3GD20 watermark event)
-volatile uint32_t watermark_delta;  // Tick delta between the last 2 watermark
-                                    // events
+volatile uint32_t last_sample_tick;  // Stores the high-res tick of the last
+                                     // interrupt (L3GD20 watermark event)
+volatile uint32_t sample_delta;      // Tick delta between the last 2 watermark
+                                     // events
 
 // L3GD20 SPI
 SPIBus bus(SPI5);
 GpioPin cs(GPIOC_BASE, 1);
-SPIBusConfig cfg;
-uint8_t buf[192];
+
+L3GD20* gyro;
 
 // Interrupt handlers
 void __attribute__((naked)) EXTI2_IRQHandler()
@@ -87,121 +79,64 @@ void __attribute__((naked)) EXTI2_IRQHandler()
 
 void __attribute__((used)) EXTI2_IRQHandlerImpl()
 {
-    uint32_t tick       = hrclock.tick();
-    watermark_delta     = tick - last_watermark_tick;
-    last_watermark_tick = tick;
+    // Current high resolution tick
+    uint32_t tick    = hrclock.tick();
+    sample_delta     = tick - last_sample_tick;
+    last_sample_tick = tick;
+
+    // Pass tick microseconds to sensor for timestamp estimation
+    if (gyro != nullptr)
+        gyro->IRQupdateTimestamp(hrclock.toIntMicroSeconds(tick));
 
     EXTI->PR |= EXTI_PR_PR2;  // Reset pending register
 }
 
 int main()
 {
-    cfg.br = SPIBaudRate::DIV_64;
-
-    {
-        FastInterruptDisableLock dLock;
-
-        RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
-        RCC->APB2ENR |= RCC_APB2ENR_SPI5EN;
+    Thread::sleep(5000);
 
-        GpioSck::mode(Mode::ALTERNATE);
-        GpioMiso::mode(Mode::ALTERNATE);
-        GpioMosi::mode(Mode::ALTERNATE);
+    configure();
 
-        GpioSck::alternateFunction(5);
-        GpioMiso::alternateFunction(5);
-        GpioMosi::alternateFunction(5);
+    gyro = new L3GD20(bus, cs, L3GD20::FullScaleRange::FS_250,
+                      L3GD20::OutPutDataRate::ODR_760, 0x03);
 
-        // Interrupt
-        GpioINT2::mode(Mode::INPUT_PULL_DOWN);
+    // gyro->setRealSampleInterval(
+    //     static_cast<uint64_t>(1000000 / SAMPLE_FREQUENCY));
 
-        cs.mode(Mode::OUTPUT);
-    }
-    // High resolution clock configuration
-    // 1.8 hours run time, 1.5 us resolution
-    hrclock.setPrescaler(127);
-    hrclock.start();
-
-    enableInterrupt();
-    cs.high();
-
-    L3GD20 gyro(bus, cs, L3GD20::FullScaleRange::FS_250,
-                L3GD20::OutPutDataRate::ODR_760, 0x03, FIFO_ENABLED,
-                FIFO_WATERMARK);
-
-    while (!gyro.init())
+    while (!gyro->init())
     {
     }
 
     Thread::sleep(500);
 
-    long long first_tick = miosix::getTick();
-
-    // Sample NUM_SAMPLES data
-    int fifo_num = 0;
     while (data_counter < NUM_SAMPLES)
     {
         long last_tick = miosix::getTick();
 
-        if (FIFO_ENABLED)
-        {
-            // Precise timestamp of the last sample in the FIFO
-            float wtm_timestamp = hrclock.toSeconds(last_watermark_tick);
-            float wtm_delta     = hrclock.toMilliSeconds(watermark_delta);
-
-            // Read the fifo
-            gyro.onSimpleUpdate();
-
-            uint8_t level =
-                gyro.getLastFifoSize();  // Number of samples in the FIFO
-
-            const array<Vec3, 32>& fifo = gyro.getLastFifo();
-
-            for (int i = 0; i < level; i++)
-            {
-                // Assign a timestamp to each sample in the FIFO
-                // Samples before the watermark are older, after the watermark
-                // are younger. Time delta between samples is
-                // (1 / SAMPLE_FREQUENCY) seconds
-                float ts = wtm_timestamp +
-                           (i - (int)FIFO_WATERMARK) / SAMPLE_FREQUENCY;
-
-                data[data_counter++] = {fifo_num,  ts,
-                                        fifo[i],   level,
-                                        wtm_delta, averageCpuUtilization()};
-                if (data_counter >= NUM_SAMPLES)
-                {
-                    break;
-                }
-            }
-            ++fifo_num;
-
-            // Wait until fifo has about 25 samples
-            Thread::sleepUntil(last_tick + 34);
-        }
-        else
-        {
-            // Sample sensor @ 500 Hz
-            gyro.onSimpleUpdate();
-
-            data[data_counter++] = {0,
-                                    (last_tick - first_tick) / 1000.0f,
-                                    *(gyro.gyroDataPtr()),
-                                    0,
-                                    0,
-                                    averageCpuUtilization()};
-            Thread::sleepUntil(last_tick + 2);
-        }
+        // Sample sensor @ 500 Hz
+        gyro->onSimpleUpdate();
+        L3GD20Data d = gyro->getLastSample();
+
+        data[data_counter++] = {d.timestamp, sample_delta, d.gyro,
+                                averageCpuUtilization()};
+
+        Thread::sleepUntil(last_tick + 1);
     }
     // Dump buffer content as CSV on the serial (might take a while)
-    printf("FIFO_num,timestamp,watermark_delta,sample_delta,x,y,z,level,cpu");
+    printf("FIFO_num,timestamp,int_delta,sample_delta,x,y,z,cpu\n");
     for (int i = 1; i < data_counter; i++)
     {
-        printf("%d,%.3f,%.3f,%.3f,%f,%f,%f,%d,%.2f\n", data[i].fifo_num,
-               data[i].timestamp * 1000, data[i].wtm_delta,
-               (data[i].timestamp - data[i - 1].timestamp) * 1000,
-               data[i].data.getX(), data[i].data.getY(), data[i].data.getZ(),
-               data[i].level, data[i].cpu);
+        // clang-format off
+         printf("%d,%llu,%llu,%llu,%f,%f,%f,%.2f\n", 
+                0,
+                data[i].timestamp, 
+                hrclock.toIntMicroSeconds(data[i].sample_delta),
+                (data[i].timestamp - data[i - 1].timestamp),
+                data[i].data.getX(), 
+                data[i].data.getY(), 
+                data[i].data.getZ(),
+                data[i].cpu);
+        // clang-format on
     }
 
     printf("\n\n\nend.\n");
@@ -211,6 +146,38 @@ int main()
     }
 }
 
+void configure()
+{
+    {
+        FastInterruptDisableLock dLock;
+
+        RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
+        RCC->APB2ENR |= RCC_APB2ENR_SPI5EN;
+
+        GpioSck::mode(Mode::ALTERNATE);
+        GpioMiso::mode(Mode::ALTERNATE);
+        GpioMosi::mode(Mode::ALTERNATE);
+
+        GpioSck::alternateFunction(5);
+        GpioMiso::alternateFunction(5);
+        GpioMosi::alternateFunction(5);
+
+        // Interrupt
+        GpioINT2::mode(Mode::INPUT_PULL_DOWN);
+
+        cs.mode(Mode::OUTPUT);
+    }
+
+    cs.high();
+
+    enableInterrupt();
+
+    // High resolution clock configuration
+    // 1.8 hours run time, 1.5 us resolution
+    hrclock.setPrescaler(127);
+    hrclock.start();
+}
+
 void enableInterrupt()
 {
     {