diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 9bcedf43deaeb1b98be2f8fe30a27aaa7505ec24..f69f0e130d360e4a90e6aa123c1466c84d0b90d2 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -20,9 +20,10 @@ build1:
   stage: build
   only:
     - master
+    - testing
   script:
     - echo "Do your build here"
-    - python sbs
+    - python3 sbs
    
 test1:
   stage: test
diff --git a/README.md b/README.md
index d813dc1cd98365931e614f9e9e95948d128627ab..03541587f6f1cbbace781036b2c250da019573ba 100644
--- a/README.md
+++ b/README.md
@@ -1,14 +1,14 @@
 ![alt text](https://avatars2.githubusercontent.com/u/8077370?s=200&v=4)
 
-Skyward Boardcore  
-[![pipeline status](https://git.skywarder.eu/r2a/skyward-boardcore/badges/master/pipeline.svg)](https://git.skywarder.eu/r2a/skyward-boardcore/commits/master)
+Skyward Boardcore
+[![pipeline status](https://git.skywarder.eu/scs/skyward-boardcore/badges/master/pipeline.svg)](https://git.skywarder.eu/scs/skyward-boardcore/commits/master)
 -------------
 
 Boardcore is a framework for developing and building missile software for custom boards with Miosix .
 
 [Miosix](https://miosix.org/) is a lightweigth OS for embedded developing which provides support for basic things such as Threads, GPIO, Time and many other. You can find our fork of the kernel here: [skyward/miosix-kernel](https://git.skywarder.eu/elc/miosix-kernel)
 
-Building is made with [SBS](https://git.skywarder.eu/r2a/skyward-boardcore/wikis/Skyward-Build-Systems-(SBS)), which was created to easily compile and reuse code for different boards. 
+Building is made with [SBS](https://git.skywarder.eu/scs/skyward-boardcore/wikis/Skyward-Build-Systems-(SBS)), which was created to easily compile and reuse code for different boards.
 
 ### Content
 
@@ -28,13 +28,22 @@ In the main folder you will find **sbs.conf** which is used to configure the bui
 
 ### Getting Started
 
-Install Python, Git and Miosix toolchain. Also, openocd and clang-format are recommended for a better experience.
+#### Dependencies
+
+* Python3
+* Git
+* Miosix toolchain
+
+Also, openocd, cppcheck and clang-format are recommended for a better experience.
+
+#### Cloning the repo
+
 
 Clone this repo with the `--recursive` option and build everything.
 ```
-git clone --recursive https://git.skywarder.eu/r2a/skyward-boardcore.git
+git clone --recursive https://git.skywarder.eu/scs/skyward-boardcore.git
 cd skyward-boardcore
-python sbs -v
+python3 sbs -v
 ```
 
 SBS will start building all the entrypoints. Depending on how many entrypoints there are, this operation can take several minutes.
@@ -43,16 +52,8 @@ Once SBS finished, check the resulting message: if every build displays an *OK*
 
 ### What's next?
 
-In the [Wiki](https://git.skywarder.eu/r2a/skyward-boardcore/wikis/home) you will find some first-steps **guides** (configuring the IDE, building a firmware etc) as well as the **coding guidelines** and some **best practices** we adopt.
-
-If you want to contribute to this repository, please read [Git Workflow](https://git.skywarder.eu/r2a/skyward-boardcore/wikis/Git-Workflow).
-
-If you just want to start messing around, try [this](https://git.skywarder.eu/r2a/skyward-boardcore/wikis/Boardcore-Quick-Start).
-
+In the [Wiki](https://git.skywarder.eu/scs/skyward-boardcore/wikis/home) you will find some first-steps **guides** (configuring the IDE, building a firmware etc) as well as the **coding guidelines** and some **best practices** we adopt.
 
-Useful links
------------
+If you want to contribute to this repository, please read [Git Workflow](https://git.skywarder.eu/scs/skyward-boardcore/wikis/Git-Workflow).
 
-* [Miosix Wiki](https://miosix.org/wiki/index.php?title=Main_Page) for the installation.
-* [Miosix Doxygen](https://miosix.org/doxygen/doxygen_k2.01/index.html) for the full documentation (classes, constants ecc).
-* [ELC Handbooks](https://git.skywarder.eu/docs/elc-internal-reports/tree/master) 
+If you just want to start messing around, try [this](https://git.skywarder.eu/scs/skyward-boardcore/wikis/Boardcore-Quick-Start).
\ No newline at end of file
diff --git a/ide/vscode/c_cpp_properties.json b/ide/vscode/c_cpp_properties.json
new file mode 100755
index 0000000000000000000000000000000000000000..abf33c87fab5ad668eab706beecadcea033a4e45
--- /dev/null
+++ b/ide/vscode/c_cpp_properties.json
@@ -0,0 +1,172 @@
+{
+    "configurations": [
+        {
+            "name": "stm32f429zi_skyward_homeone",
+            "includePath": [
+                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_homeone",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_homeone",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix",
+                "${workspaceFolder}/libs",
+                "${workspaceFolder}/src/shared",
+                "${workspaceFolder}/src/tests",
+                "${workspaceFolder}"
+            ],
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+
+            "defines": [
+                "DEBUG",
+                "_ARCH_CORTEXM4_STM32F4",
+                "_BOARD_STM32F429ZI_SKYWARD_HOMEONE",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_homeone",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
+                "_MIOSIX",
+                "__cplusplus=201103L"
+            ],
+     
+            "cStandard": "c11",
+            "cppStandard": "c++11",
+
+            "browse": {
+                "path" : [
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_homeone/interfaces-impl",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_homeone",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_homeone",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_homeone",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/interfaces",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/kernel",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/util",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/e20",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/filesystem",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/stdlib_integration",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/*",
+                    "${workspaceFolder}/libs/mavlink_skyward_lib",               
+                    "${workspaceFolder}/libs/tscpp",
+                    "${workspaceFolder}/src/shared",
+                    "${workspaceFolder}/src/tests",
+                    "${workspaceFolder}/*",
+                    "${workspaceFolder}/src",
+                    "${workspaceFolder}/*"
+                ],
+                "limitSymbolsToIncludedHeaders": true
+            }
+        },
+        {
+            "name": "stm32f429zi_skyward_death_stack",
+            "includePath": [
+                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix",
+                "${workspaceFolder}/libs",
+                "${workspaceFolder}/src/shared",
+                "${workspaceFolder}/src/tests",
+                "${workspaceFolder}"
+            ],
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+
+            "defines": [
+                "DEBUG",
+                "_ARCH_CORTEXM4_STM32F4",
+                "_BOARD_stm32f429zi_skyward_death_stack",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
+                "_MIOSIX",
+                "__cplusplus=201103L"
+            ],
+     
+            "cStandard": "c11",
+            "cppStandard": "c++11",
+
+            "browse": {
+                "path" : [
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack/interfaces-impl",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/interfaces",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/kernel",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/util",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/e20",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/filesystem",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/stdlib_integration",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/*",
+                    "${workspaceFolder}/libs/mavlink_skyward_lib",               
+                    "${workspaceFolder}/libs/tscpp",
+                    "${workspaceFolder}/src/shared",
+                    "${workspaceFolder}/src/tests",
+                    "${workspaceFolder}/*",
+                    "${workspaceFolder}/src"
+                ],
+                "limitSymbolsToIncludedHeaders": true
+            }
+        },
+        {
+            "name": "stm32f429zi_skyward_rogallina",
+            "includePath": [
+                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_rogallina",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_rogallina",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix",
+                "${workspaceFolder}/libs",
+                "${workspaceFolder}/src/shared",
+                "${workspaceFolder}/src/tests",
+                "${workspaceFolder}"
+            ],
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+
+            "defines": [
+                "DEBUG",
+                "_ARCH_CORTEXM4_STM32F4",
+                "_BOARD_STM32F429ZI_SKYWARD_HOMEONE",
+                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_homeone",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
+                "_MIOSIX",
+                "__cplusplus=201103L"
+            ],
+     
+            "cStandard": "c11",
+            "cppStandard": "c++11",
+
+            "browse": {
+                "path" : [
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_rogallina/interfaces-impl",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_rogallina",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_rogallina",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_rogallina",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/interfaces",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/kernel",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/util",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/e20",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/filesystem",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/stdlib_integration",
+                    "${workspaceFolder}/libs/miosix-kernel/miosix/*",
+                    "${workspaceFolder}/libs/mavlink_skyward_lib",   
+                    "${workspaceFolder}/libs/mxgui",                   
+                    "${workspaceFolder}/libs/tscpp",
+                    "${workspaceFolder}/libs/*",     
+                    "${workspaceFolder}/src/shared",
+                    "${workspaceFolder}/src/tests",
+                    "${workspaceFolder}/*",
+                    "${workspaceFolder}/src",
+                    "${workspaceFolder}/*"
+                ],
+                "limitSymbolsToIncludedHeaders": true
+            }
+        }
+        
+    ],
+    "version": 4
+}
\ No newline at end of file
diff --git a/ide/vscode/launch.json b/ide/vscode/launch.json
new file mode 100644
index 0000000000000000000000000000000000000000..f07981bfab285ff228026edb5840654b0d08f3f9
--- /dev/null
+++ b/ide/vscode/launch.json
@@ -0,0 +1,30 @@
+{
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "cwd": "${workspaceRoot}",
+            "executable": "${workspaceFolder}/bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.elf",
+            "name": "ST-LINK V2 (current entrypoint)",
+            "request": "attach",
+            "type": "cortex-debug",
+            "servertype": "openocd",
+            "device": "STM32F429ZI",
+            "svdFile": "/home/luca/drive/Skyward/STM32F429.svd",
+            "configFiles": [
+                "/home/luca/test.cfg"
+            ]
+        },
+        {
+            "cwd": "${workspaceRoot}",
+            "executable": "${workspaceFolder}/bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.elf",
+            "name": "ST-LINK V1 (current entrypoint)",
+            "request": "attach",
+            "type": "cortex-debug",
+            "servertype": "openocd",
+            "device": "STM32F429ZI",
+            "configFiles": [
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack/death_stack.cfg"
+            ]
+        }
+    ]
+}
\ No newline at end of file
diff --git a/ide/vscode/tasks.json b/ide/vscode/tasks.json
new file mode 100755
index 0000000000000000000000000000000000000000..3afed1afbf7ea2f36e4c773f52487bdfe1f46f34
--- /dev/null
+++ b/ide/vscode/tasks.json
@@ -0,0 +1,111 @@
+{
+  "version": "2.0.0",
+  "tasks": [
+    {
+      "label": "Show Workspace Folder",
+      "type": "shell",
+      "windows": {
+        "command": "echo ${workspaceFolder}"
+      },
+      "problemMatcher": []
+    },
+    {
+      "label": "CLEAN",
+      "type": "shell",
+      "windows": {
+        "command": "python sbs -c"
+      },
+      "linux": {
+        "command": "./sbs -c"
+      },
+      "problemMatcher": "$gcc"
+    },
+    {
+      "label": "BUILD all",
+      "type": "shell",
+      "windows": {
+        "command": "python sbs"
+      },
+      "linux": {
+        "command": "./sbs"
+      },
+      "problemMatcher": "$gcc"
+    },
+    {
+      "label": "BUILD current-entrypoint",
+      "type": "shell",
+      "windows": {
+        "command": "python sbs -v -n -b ${fileBasenameNoExtension}"
+      },
+      "linux": {
+        "command": "./sbs -v -b ${fileBasenameNoExtension}"
+      },
+      "problemMatcher": "$gcc"
+    },
+    {
+      "label": "RUN current-entrypoint",
+      "type": "shell",
+      "windows": {
+        "command": "ST-LINK_CLI.exe -P bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000 -V -Rst"
+      },
+      "linux": {
+        "command": "st-flash write bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000"
+      },
+      "problemMatcher": []
+    },
+    {
+      "label": "BUILD+RUN current-entrypoint",
+      "type": "shell",
+      "windows": {
+        "command": "ST-LINK_CLI.exe -P bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000 -V -Rst"
+      },
+      "linux": {
+        "command": "sleep 1;st-flash write bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.bin 0x8000000"
+      },
+      "problemMatcher": [],
+      "dependsOn": [
+        "BUILD current-entrypoint"
+      ],
+      "group": {
+        "kind": "build",
+        "isDefault": true
+      }
+    },
+    {
+      "label": "BUILD tests-catch",
+      "type": "shell",
+      "windows": {
+        "command": "python sbs -v -n -b tests-catch"
+      },
+      "linux": {
+        "command": "./sbs -v -b tests-catch"
+      },
+      "problemMatcher": "$gcc"
+    },
+    {
+      "label": "RUN tests-catch",
+      "type": "shell",
+      "windows": {
+        "command": "ST-LINK_CLI.exe -P bin/tests-catch/tests-catch.bin 0x8000000 -V"
+      },
+      "linux": {
+        "command": "st-flash write bin/tests-catch/tests-catch.bin 0x8000000"
+      },
+      "problemMatcher": []
+    },
+    {
+      "type": "shell",
+      "label": "arm-miosix-eabi-g++ build active file",
+      "command": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+      "args": [
+        "-g",
+        "${file}",
+        "-o",
+        "${fileDirname}/${fileBasenameNoExtension}"
+      ],
+      "options": {
+        "cwd": "/opt/arm-miosix-eabi/bin"
+      }
+    }
+  ]
+}
\ No newline at end of file
diff --git a/libs/miosix-kernel b/libs/miosix-kernel
index a89006f621fe7e6937b1102b8a8ef35e5f486f6d..9096c209182fc1c86a1f4f33c9a1546fdad4e0b3 160000
--- a/libs/miosix-kernel
+++ b/libs/miosix-kernel
@@ -1 +1 @@
-Subproject commit a89006f621fe7e6937b1102b8a8ef35e5f486f6d
+Subproject commit 9096c209182fc1c86a1f4f33c9a1546fdad4e0b3
diff --git a/sbs b/sbs
index 8a4449230b92485761f59ee3d469187fa9c4fda8..d159ba3a51bb38c2280f2f0853a1aa75da16d187 100755
--- a/sbs
+++ b/sbs
@@ -4,17 +4,17 @@
 #
 # Copyright (c) 2015-2019 Skyward Experimental Rocketry
 # Authors: Alain Carlucci, 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
@@ -82,7 +82,7 @@ def printBanner():
     print("+----------------------------------|___/-------------------v2.0-+")
 
 #
-# Colorized output helper functions 
+# Colorized output helper functions
 # NOTE: I don't know why but an extra \n is added at the end of each line,
 # so we have to use replace
 #
@@ -110,23 +110,25 @@ def print_make_output(text):
 #
 def configCmdParser():
     parser = OptionParser()
-    parser.add_option("-c", "--clean", help="Run a 'make clean'", dest="clean", 
+    parser.add_option("-c", "--clean", help="Run a 'make clean'", dest="clean",
         action="store_true")
     parser.add_option("-b", "--build", help="Build a specific entrypoint or test", dest="board")
     parser.add_option("-l", "--list", help="List all possible configurations", dest="list",
         default=False, action='store_true')
-    parser.add_option("-t", "--all-test", help="Build all tests", dest="all_tests", 
+    parser.add_option("-t", "--all-test", help="Build all tests", dest="all_tests",
         default=False, action='store_true')
-    parser.add_option("-e", "--all-entrypoint", help="Build all entrypoints", dest="all_entry", 
+    parser.add_option("-e", "--all-entrypoint", help="Build all entrypoints", dest="all_entry",
         default=False, action='store_true')
-    parser.add_option("-g", "--gen-faults", help="Generate fault list header files and exit", 
-        dest="genhdr", default=False, action='store_true')
-    parser.add_option("-v", "--verbose", help="Print a verbose output", dest="log", 
+#    parser.add_option("-g", "--gen-faults", help="Generate fault list header files and exit",
+#        dest="genhdr", default=False, action='store_true')
+    parser.add_option("-v", "--verbose", help="Print a verbose output", dest="log",
         action="store_true")
-    parser.add_option("-j", "--jobs", 
-        help="Specifies the number of jobs (commands) to run simultaneously.", 
+    parser.add_option("", "--lint", help="Run the linter", dest="lint",
+        action="store_true")
+    parser.add_option("-j", "--jobs",
+        help="Specifies the number of jobs (commands) to run simultaneously.",
         type="int", dest="JOBS", default=8)
-    parser.add_option("-n", "--no-colors", help="Don't print colors in the output", dest="colors", 
+    parser.add_option("-n", "--no-colors", help="Don't print colors in the output", dest="colors",
         default=True,  action="store_false")
     return parser
 
@@ -153,14 +155,14 @@ def parseConf(path):
         elif stype == 'srcfiles':
             srcfiles[i] = [x.strip() for x in conf.get(i, 'Files').split("\n")]
         elif stype == 'board':
-            entrypoints[i] = {   'id': conf.get(i, 'BoardId'), 
+            entrypoints[i] = {   'id': conf.get(i, 'BoardId'),
                             'bin': conf.get(i, 'BinName'),
                             'defines': conf.get(i, 'Defines'),
                             'files': [entrypoint_mask % conf.get(i, 'Main').strip()] +
                              [x.strip() for x in conf.get(i, 'Include').split(' ')]
-                        }    
+                        }
         elif stype == 'test':
-            tests[i] = {   'id': conf.get(i, 'BoardId'), 
+            tests[i] = {   'id': conf.get(i, 'BoardId'),
                             'bin': conf.get(i, 'BinName'),
                             'defines': conf.get(i, 'Defines') + " -I" + projconf['TESTS_PATH'],
                             'files': [test_mask % conf.get(i, 'Main').strip()] +
@@ -215,10 +217,10 @@ def build_makefile(template, board, bname):
 
     for incl in incllist:
         rmap["PROJECT_INCLUDES"] += incl.strip() + " "
-    
+
     for lib in liblist:
         rmap["PROJECT_LIBS"] += lib.strip() + " "
-    
+
     for sd in subdirs:
         rmap["PROJECT_SUBDIRS"] += sd.strip() + " "
 
@@ -232,10 +234,10 @@ def build_makefile(template, board, bname):
 #
 # Generate fault counter headers
 #
-def genFaultCounters():
-    d1 = "data/fault_list.csv"
-    d2 = SBS_BASE + "/src/shared/diagnostic/FaultCounterData.h"
-    os.system("python %s/scripts/gen_fault_headers.py %s %s" %(SBS_BASE,d1,d2))
+#def genFaultCounters():
+#    d1 = "data/fault_list.csv"
+#    d2 = SBS_BASE + "/src/shared/diagnostic/FaultCounterData.h"
+#    os.system("python %s/scripts/gen_fault_headers.py %s %s" %(SBS_BASE,d1,d2))
 
 #
 # Import srcfiles groups from another .conf file.
@@ -282,9 +284,15 @@ if len(make_template) == 0:
 #
 
 # Fault headers generation
-if options.genhdr == True:
-    genFaultCounters()
-    exit(0)
+# if options.genhdr == True:
+#     genFaultCounters()
+#     exit(0)
+
+# Linter
+if options.lint == True:
+    print("[SBS] Executing linter.sh", flush=True)
+    os.system("%s/scripts/linter.sh %s" % (projconf['SBS_BASE'], projconf['SRC_PATH']) )
+    sys.exit(0)
 
 # List
 if options.list == True:
@@ -326,16 +334,16 @@ os.mkdir('build')
 
 for i in mainfiles:
     printout('[SBS] %s %s' % (action[1],i), WHITE, BOLD)
-    
+
     with open('build/%s' % i, 'w') as f:
         f.write(build_makefile(make_template, mainfiles[i], i))
 
     # stdout printed only in verbose
     if options.log == True:
-        p = subprocess.Popen(['make', '-j', str(options.JOBS), '-f', 'build/%s' % i]
+        p = subprocess.Popen(['make','-j', str(options.JOBS), '-f', 'build/%s' % i]
                                 + cleanparam,
                                 universal_newlines=True,
-                                #stdout=subprocess.PIPE, 
+                                #stdout=subprocess.PIPE,
                                 stderr=subprocess.PIPE,
                                 bufsize=1)
     else:
@@ -362,4 +370,4 @@ for i in mainfiles:
 
 # Clean
 if options.clean == True:
-    shutil.rmtree('build',ignore_errors=True, onerror=None)
+    shutil.rmtree('build',ignore_errors=True, onerror=None)
\ No newline at end of file
diff --git a/sbs.conf b/sbs.conf
index 6f9a8809c1443e5748dbf62b4640e14be9ac24fe..58769fb570789f0dd4485b02f5f62889560abdaf 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -51,6 +51,7 @@
 #stm32f429zi_skyward_homeone
 #stm32f401re_nucleo
 #stm32f103c8_skyward_aldeeran
+#stm32f407vg_skyward_tortellino
 
 
 # Syntax of this file:
@@ -76,9 +77,9 @@ ENTRY_PATH:       src/entrypoints
 TESTS_PATH:       src/tests
 SRC_PATH:         src/shared
 SBS_BASE:         .
-PROJECT_INCLUDES:  
-PROJECT_SUBDIRS:  
-PROJECT_LIBS:     
+PROJECT_INCLUDES:
+PROJECT_SUBDIRS:
+PROJECT_LIBS:
 
 #-----------------------------------#
 #          List of Sources          #
@@ -187,7 +188,7 @@ Main:       config-dsgamma
 Type:       board
 BoardId:    stm32f429zi_stm32f4discovery
 BinName:    kernel-testsuite
-Include:    
+Include:
 Defines:
 Main:       kernel-testsuite
 
@@ -240,7 +241,7 @@ Main:       test-hsm
 Type:       test
 BoardId:    stm32f429zi_stm32f4discovery
 BinName:    test-interrupt
-Include:    
+Include:
 Defines:    -DDEBUG
 Main:       test-interrupt
 
@@ -256,7 +257,7 @@ Main:       logger/test-logger
 Type:       test
 BoardId:    stm32f429zi_stm32f4discovery
 BinName:    test-kalman-benchmark
-Include:    
+Include:
 Defines:
 Main:       kalman/test-kalman-benchmark
 
@@ -294,6 +295,14 @@ Include:    %shared
 Defines:
 Main:       drivers/test-mpu9250
 
+[test-lsm]
+Type:       test
+BoardId:    stm32f429zi_skyward_death_stack
+BinName:    test-lsm
+Include:    %shared
+Defines:
+Main:       drivers/test-lsm
+
 [test-timer]
 Type:       test
 BoardId:    stm32f429zi_stm32f4discovery
@@ -331,7 +340,7 @@ Type:       test
 BoardId:    stm32f429zi_stm32f4discovery
 BinName:    test-IMU
 Include:    %shared
-Defines:    
+Defines:
 Main:       drivers/test-IMU
 
 #[test-mavlink-multi]
diff --git a/scripts/cppcheck_suppressions_list.txt b/scripts/cppcheck_suppressions_list.txt
deleted file mode 100644
index 84fbc0df253990712eeec40ac270a53a17228317..0000000000000000000000000000000000000000
--- a/scripts/cppcheck_suppressions_list.txt
+++ /dev/null
@@ -1,2 +0,0 @@
-*:src/tests/*
-
diff --git a/scripts/linter.sh b/scripts/linter.sh
index 79792ccfa247a75ddccfc2bb96a2db7dccbe98ce..70dd0adb8d12a393cc67d72cc2051353b27c2b22 100755
--- a/scripts/linter.sh
+++ b/scripts/linter.sh
@@ -1,38 +1,18 @@
 #!/bin/bash
 
-declare -i RESULT=0
+echo '[Linter] Executing cppcheck'
+cppcheck  --template=gcc -q --std=c++11 --enable=all \
+            --suppress=unusedFunction --suppress=missingInclude --suppress=noExplicitConstructor \
+            "$@" 2>&1 | awk '
+                function color(c,s) {
+                        printf("\033[%dm%s\033[0m\n",30+c,s)
+                }
+                /warning/ {color(1,$0);next}
+                /style/ {color(2,$0);next}
+                /performance/ {color(3,$0);next}
+                /information/ {color(4,$0);next}
+                /portability/ {color(5,$0);next}
+                {print}
+                '
 
-function check {
-    B=$(eval "$3")
-    A=$?
-    echo -n "[LINTER] $2... "
-
-    if [ "$1" == "OUT" ]; then
-        if [ ${#B} -gt 0 ]; then
-            A=$1
-        fi;
-    fi;
-    if [ "$A" == "$1" ]; then
-        echo -e "FAIL\n------------ OUTPUT ------------"
-        echo "$B"
-        echo "--------------------------------"
-        return 1
-    else
-        echo "OK"
-        return 0
-    fi;
-}
-
-#check 0 "Checking for files with lines longer than 80 characters" 'egrep --include="*.cpp" --include="*.h" -nr ".{81}" src/' 
-check 0 "Checking for files with lines longer than 120 characters" 'egrep --include="*.cpp" --include="*.h" -nr ".{121}" src/' 
-RESULT+=$?
-check OUT "Checking for files having \n\n\n" "grep -HPcrz '(\\r?\\n){4,}' src | egrep -v ':0$' | cut -d ':' -f 1"
-check OUT "Checking for files not having the copyright" "grep -rL '* Permission is hereby granted, free of charge' src/"
-check OUT "Checking for tabulations instead of spaces" "grep -Pr '\t' src"
-check OUT "MMP wants his full name" "grep -rl 'Matteo Piazzolla' src/"
-check OUT "Launching cppcheck" "cppcheck  --suppressions-list=./scripts/cppcheck_suppressions_list.txt --template=gcc -q --suppress=unusedFunction --suppress=missingInclude --std=c++11 --enable=all src/ 2>&1"
-RESULT+=$?
-check OUT "Checking for using namespace in header files" "grep -rl 'using namespace' src | egrep '.h(pp)?$'"
-RESULT+=$?
-
-exit 0
+#exit 0
diff --git a/src/shared/diagnostic/NewLogger.h b/src/shared/diagnostic/NewLogger.h
new file mode 100644
index 0000000000000000000000000000000000000000..ef14f373559a4e3bbc627670351216d76e17f019
--- /dev/null
+++ b/src/shared/diagnostic/NewLogger.h
@@ -0,0 +1,177 @@
+/* Copyright (c) 2015-2017 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.
+ */
+
+#ifndef SRC_SHARED_DIAGNOSTIC_NEWLOGGER_H_
+#define SRC_SHARED_DIAGNOSTIC_NEWLOGGER_H_
+
+#include <iostream>
+#include <string>
+#include <sstream>
+#include <ctime>
+#include <iomanip>
+
+namespace logging
+{
+
+enum class LogLevel : int
+ {
+  INFO = 0,
+  NOTIFY = 1,
+  WARNING = 2,
+  ERROR = 3,
+  CRITICAL = 4
+};
+
+template <typename T>
+std::string hex(T val)
+{
+  std::stringstream stream;
+  stream << std::hex << val;
+  std::string result( stream.str() );
+  return result;
+}
+
+class Logger
+{
+ public:
+  Logger() : out_stream_(&std::cout)
+  {
+
+  }
+
+  Logger(std::ostream* ostream) : out_stream_(ostream)
+  {
+
+  }
+
+  void setOstream(std::ostream* new_ostream)
+  {
+    out_stream_ = new_ostream;
+  }
+
+  template<typename ... Args>
+  void log(LogLevel level, std::string tag, Args ...args) const
+  {
+    if (level < log_level_)
+      return;
+
+    std::stringstream stream;
+    stream << getStringPrefix(level) << formatTag(tag) << " ";
+    _printOnSStream(&stream, args...);
+    *out_stream_ << stream.str() << std::endl;
+  }
+
+  template<typename ... Args>
+  void info(std::string tag, Args ... args) const
+  {
+    log(LogLevel::INFO, tag, args...);
+  }
+
+  template<typename ... Args>
+  void notify(std::string tag, Args ... args) const
+  {
+    log(LogLevel::NOTIFY, tag, args...);
+  }
+
+  template<typename ... Args>
+  void warning(std::string tag, Args ... args) const
+  {
+    log(LogLevel::WARNING, tag, args...);
+  }
+
+  template<typename ... Args>
+  void error(std::string tag, Args ... args) const
+  {
+    log(LogLevel::ERROR, tag, args...);
+  }
+
+  template<typename ... Args>
+  void critical(std::string tag, Args ... args) const
+  {
+    log(LogLevel::CRITICAL, tag, args...);
+  }
+
+ private:
+
+  std::string getStringPrefix(LogLevel level) const
+  {
+    //Text is colored and formatted using Ansi Escape Codes:
+    //https://en.wikipedia.org/wiki/ANSI_escape_code
+
+    switch (level)
+    {
+      case LogLevel::INFO:
+        return "\033[37m[INFO]\033[0m     ";
+        break;
+      case LogLevel::NOTIFY:
+        return "\033[32m[NOTIFY]\033[0m   ";
+        break;
+      case LogLevel::WARNING:
+        return "\033[33m[WARNING]\033[0m  ";
+        break;
+      case LogLevel::ERROR:
+        return "\033[1;31m[ERROR]\033[0m    ";
+        break;
+      case LogLevel::CRITICAL:
+        return "\033[1;41;37m[CRITICAL]\033[0m ";
+        break;
+    }
+    return "";
+  }
+
+  static std::string formatTag(std::string tag)
+  {
+    static const unsigned int maxlen = 14;
+    if(tag.length() > maxlen)
+      tag = tag.substr(0, maxlen);
+
+    tag + tag + ": ";
+
+    //Add trailing whitespace
+    std::string whitespace(maxlen - tag.length() + 2, ' ');
+    tag = tag + whitespace;
+
+    return tag;
+  }
+
+  void _printOnSStream(std::stringstream*) const
+  {
+  }
+
+  template<typename FirstArg, typename ... Args>
+  void _printOnSStream(std::stringstream* stream, const FirstArg& arg,
+                       Args ...args) const
+  {
+    *stream << arg;
+    _printOnSStream(stream, args...);
+  }
+
+  LogLevel log_level_ = LogLevel::INFO;
+  std::ostream* out_stream_;
+};
+
+//Default logger on cout
+static const Logger logger;
+
+}   //namespace logger
+
+#endif /* SRC_SHARED_DIAGNOSTIC_NEWLOGGER_H_ */
diff --git a/src/shared/drivers/BusTemplate.h b/src/shared/drivers/BusTemplate.h
index 208e9212cad8e1d26494f820009f3d074c065c61..0652a840bf017fa009fbc6883e85947ac5454318 100644
--- a/src/shared/drivers/BusTemplate.h
+++ b/src/shared/drivers/BusTemplate.h
@@ -192,6 +192,7 @@ private:
             getSPIAddr(N)->CR1=SPI_CR1_SSM  //No HW cs
                             | SPI_CR1_SSI
                             | SPI_CR1_SPE  //SPI enabled
+                            | SPI_CR1_BR_0
                             | SPI_CR1_BR_1
                             | SPI_CR1_BR_2
                             | SPI_CR1_MSTR; 
diff --git a/src/shared/drivers/memory/FlashController.h b/src/shared/drivers/memory/FlashController.h
new file mode 100644
index 0000000000000000000000000000000000000000..7d123ba22167b6572e8310a0c82f082825b43870
--- /dev/null
+++ b/src/shared/drivers/memory/FlashController.h
@@ -0,0 +1,656 @@
+//
+//  FlashController.hpp
+//  FlashController
+//
+//  Created by Matteo Piazzolla on 27/05/17.
+//  Copyright © 2017 Matteo Piazzolla. All rights reserved.
+//
+
+#ifndef FLASHCONTROLLER_H
+#define FLASHCONTROLLER_H
+
+#include <drivers/memory/FlashDriverInclude.h>
+
+#include <miosix.h>
+#include <Singleton.h>
+#include <diagnostic/FaultCounter.h>
+#include <diagnostic/NewLogger.h>
+
+using logging::logger;
+
+//Forward declaration
+namespace testing
+{
+namespace flashmemorytests
+{
+template<typename >
+class FlashTest;
+}
+}
+
+namespace flashmemory
+{
+
+/*
+ 16 pagine da 256 byte
+ 1000000 pagine in un banco
+ 62500 sottosettori
+ */
+static const uint16_t CONTROLLER_VERSION = 1;
+static const uint16_t BOARD_ID = 0xAB;  //TBD
+
+//BLOCK_SIZE deve essere un divisore di SUBSECTOR_SIZE
+static const uint32_t BLOCK_SIZE = PAGE_SIZE;
+
+/** Number of flash data blocks that fit in each sector.
+ *  First block of each sector is reserved for the header.
+ *  In the first sector, the entire first subsector is reserved for the header.
+ */
+static const uint32_t BLOCKS_PER_SECTOR = SECTOR_SIZE / BLOCK_SIZE;
+static const uint32_t BLOCKS_NUM = BLOCKS_PER_SECTOR * SECTORS_NUM;
+
+static const uint32_t FIRST_BOOT_KEY = 0xF135B007;
+static const uint32_t READ_ONLY_KEY = 0x3EAD0271;
+
+//How many sectors should we attempt to write in before we give up?
+static const int MAX_SECTOR_ITERATIONS = 50;
+
+struct FlashHeader
+{
+  const uint16_t version = CONTROLLER_VERSION;
+  const uint16_t board_id = BOARD_ID;
+
+  uint32_t first_boot = FIRST_BOOT_KEY;
+  uint32_t start_index = 0;
+  uint32_t read_only = 0;
+
+  bool operator==(const FlashHeader& other) const
+  {
+    return memcmp(this, &other, sizeof(FlashHeader)) == 0;
+  }
+
+  bool operator!=(const FlashHeader& other) const
+  {
+    return memcmp(this, &other, sizeof(FlashHeader)) != 0;
+  }
+};
+
+static const uint8_t SECTOR_EMPTY = 0x55;
+static const uint8_t SECTOR_WRITTEN = 0x44;
+static const uint8_t SECTOR_CORRUPT = 0x00;
+
+struct SectorHeader
+{
+  uint8_t content = SECTOR_CORRUPT;  //Either one of SECTOR_EMPTY,
+  //SECTOR_WRITTEN, SECTOR_CORRUPT
+  bool operator==(const SectorHeader& other) const
+  {
+    return memcmp(this, &other, sizeof(SectorHeader)) == 0;
+  }
+};
+
+static const uint32_t FLASH_BUFFER_SIZE = (BLOCK_SIZE)
+    - (sizeof(uint32_t) * 2 + sizeof(uint16_t));
+
+struct FlashDataBlock
+{
+  uint32_t id;
+  uint32_t timestamp;
+  uint8_t buffer[FLASH_BUFFER_SIZE];
+  uint16_t crc;
+
+  bool operator==(const FlashDataBlock& other) const
+  {
+    return memcmp(this, &other, sizeof(FlashDataBlock)) == 0;
+  }
+
+  bool operator!=(const FlashDataBlock& other) const
+  {
+    return memcmp(this, &other, sizeof(FlashDataBlock)) != 0;
+  }
+};
+
+template<typename MemoryBus>
+class FlashController : Singleton<FlashController<MemoryBus>>
+{
+  friend class Singleton<FlashController<MemoryBus>>;
+
+  template<typename >
+  friend class testing::flashmemorytests::FlashTest;
+
+  typedef FlashController<MemoryBus> FlashControllerType;
+  typedef FlashDriver<MemoryBus> FlashDriverType;
+
+ public:
+  enum class Status
+  {
+    WRITE_READY = 0,
+    READ_ONLY = 1,
+    INITIALIZATION_ERROR = 2,
+    WRITE_ERROR = 3
+  };
+
+  void init()
+  {
+    status_ = Status::INITIALIZATION_ERROR;
+    block_index_ = SUBSECTOR_SIZE / BLOCK_SIZE;
+
+    FlashHeader header;
+
+    if (!readFlashHeader(&header))
+    {
+      logger.critical(logtag(),
+                      "Cannot read flash header.");
+      sFaultCounterMgr->Increment(Fault::F_MASTER_FLASH_CNTRL_FAULT);
+      return;
+    }
+
+    if (header.read_only == READ_ONLY_KEY)
+    {
+      flash_->setReadOnly();
+      status_ = Status::READ_ONLY;
+    } else
+    {
+      if (header.first_boot == FIRST_BOOT_KEY)
+      {
+        //Update the first boot key
+        header.first_boot = 0;
+
+        if (!writeFlashHeader(header, false))
+        {
+          logger.critical(logtag(),
+                                "Could not update first boot value.");
+          sFaultCounterMgr->Increment(Fault::F_MASTER_FLASH_CNTRL_FAULT);
+          return;
+        }
+
+        if (header.start_index >= block_index_)
+        {
+          block_index_ = header.start_index;
+        }
+        status_ = Status::WRITE_READY;
+      } else
+      {
+        if (recoverFromReboot())
+        {
+          status_ = Status::WRITE_READY;
+        } else
+        {
+          logger.critical(logtag(), "Couldn't find sector to write in.");
+          sFaultCounterMgr->Increment(Fault::F_MASTER_FLASH_CNTRL_FAULT);
+        }
+      }
+    }
+  }
+
+  FlashController::Status getStatus()
+  {
+    return status_;
+  }
+
+  uint32_t getBlockIndex()
+  {
+    return block_index_;
+  }
+
+  /**
+   * @brief Writes a block onto the flash memory.
+   *
+   * @param block The block to be written on the flash
+   */
+  bool writeDataBlock(FlashDataBlock* block_buf)
+  {
+
+    if (status_ != Status::WRITE_READY)
+    {
+      logger.error(logtag(), "Writing is not enabled.");
+      return false;
+    }
+
+    if (block_index_ >= BLOCKS_NUM)
+    {
+      logger.critical(logtag(), "Memory is full.");
+      sFaultCounterMgr->Increment(Fault::F_MASTER_FLASH_CNTRL_FAULT);
+      status_ = Status::WRITE_ERROR;
+      return false;
+    }
+
+    //Enforce size <= BLOCK_SIZE
+    size_t size = sizeof(FlashDataBlock) < BLOCK_SIZE ?
+        sizeof(FlashDataBlock) : BLOCK_SIZE;
+
+    //Beginning of a new sector. Check if we can write in it and update its header
+    if (block_index_ % BLOCKS_PER_SECTOR == 0)
+    {
+      if(!gotoNewSector()){
+        logger.critical(logtag(), "Could not find sector to write in.");
+        status_ = Status::WRITE_ERROR;
+        sFaultCounterMgr->Increment(Fault::F_MASTER_FLASH_CNTRL_FAULT);
+        return false;
+      }
+    }
+
+    uint8_t result;
+    flash_->write(&result, block_index_ * BLOCK_SIZE,
+                  reinterpret_cast<uint8_t*>(block_buf), size);
+    block_index_++;
+
+    return result == RESULT_OK;
+  }
+
+  static bool readFlashHeader(FlashHeader* header)
+  {
+    uint8_t result;
+    FlashDriverType* flash = Singleton<FlashDriverType>::getInstance();
+    flash->read(&result, 0, (uint8_t*) header, sizeof(FlashHeader));
+    return result == RESULT_OK;
+  }
+
+  /**
+   * Saves (and overwrites) the header in the first address on the memory.
+   * @param header
+   */
+  static bool writeFlashHeader(const FlashHeader& header, bool erase = true)
+  {
+    uint8_t result;
+    FlashDriverType* flash = Singleton<FlashDriverType>::getInstance();
+    if (erase)
+    {
+      flash->enableErase();
+      flash->eraseSubsector(&result, 0);
+      if (result != OpResultFlags::RESULT_OK)
+      {
+        return false;
+      }
+    }
+    uint32_t size =
+        sizeof(FlashHeader) > SUBSECTOR_SIZE ? SUBSECTOR_SIZE :
+                                               sizeof(FlashHeader);
+
+    flash->writeAndCheck(&result, 0, (uint8_t*) (&header), size);
+    return result == RESULT_OK;
+  }
+
+  static bool readBlock(uint32_t block_index, uint8_t* buffer)
+  {
+    uint8_t result;
+    FlashDriverType* flash = Singleton<FlashDriverType>::getInstance();
+
+    flash->read(&result, block_index * BLOCK_SIZE, buffer, BLOCK_SIZE);
+    return result == RESULT_OK;
+  }
+
+  static bool readDataBlock(uint32_t block_index, FlashDataBlock* block)
+  {
+    //Flash header & sector headers are not *data* blocks.
+    if(block_index % BLOCKS_PER_SECTOR == 0
+        || block_index < SUBSECTOR_SIZE / BLOCK_SIZE)
+    {
+      return false;
+    }
+
+    readBlock(block_index, reinterpret_cast<uint8_t*>(block));
+    return true;
+  }
+
+  /**
+   * Saves (and overwrites) the header in the first address on the memory.
+   * @param header
+   */
+  static bool writeSectorHeader(uint32_t sector, const SectorHeader& header)
+  {
+    uint8_t result;
+    FlashDriverType* flash = Singleton<FlashDriverType>::getInstance();
+
+    uint32_t size =
+        sizeof(SectorHeader) > BLOCK_SIZE ? BLOCK_SIZE : sizeof(SectorHeader);
+
+    flash->writeAndCheck(&result, sector * SECTOR_SIZE, (uint8_t*) (&header),
+                         size);
+
+    return result == RESULT_OK;
+  }
+
+  static bool readSectorHeader(uint32_t sector, SectorHeader* header)
+  {
+    return readSectorHeader(sector, (uint8_t*) header);
+  }
+
+  static bool readSectorHeader(uint32_t sector, uint8_t* header_buff)
+  {
+    uint8_t result;
+    FlashDriverType* flash = Singleton<FlashDriverType>::getInstance();
+    flash->read(&result, SECTOR_SIZE * sector, header_buff,
+                sizeof(SectorHeader));
+    return result == RESULT_OK;
+  }
+
+  static bool format()
+  {
+    uint8_t result;
+    logger.warning(logtag(), "THE MEMORY WILL BE FORMATTED IN 5 SECONDS");
+    //Thread::sleep(5000);
+    logger.info(logtag(), "Erasing...");
+    //TODO: Flash leds for a few seconds as a warning
+    eraseMemory(&result);
+    if (result != OpResultFlags::RESULT_OK)
+    {
+      logger.error(logtag(), "Error: result=", logging::hex(result));
+      return false;
+    }
+    logger.info(logtag(), "Writing flash header...");
+    FlashHeader header = { };
+    header.first_boot = FIRST_BOOT_KEY;
+    //header.magic_word = MAGIC_WORD;
+
+    if (!writeFlashHeader(header, false))
+    {
+      logger.error(logtag(), "Error: result=", logging::hex(result));
+      return false;
+    }
+    logger.info(logtag(), "Writing sector headers...");
+    SectorHeader sheader;
+    sheader.content = SECTOR_EMPTY;
+
+    for (uint32_t sector = 1; sector < SECTORS_NUM; sector++)
+    {
+      if (!writeSectorHeader(sector, sheader))
+      {
+        logger.error(logtag(), "Error: result=", logging::hex(result),
+                     " sector: ", sector);
+        return false;
+      }
+    }
+    logger.info(logtag(), "Success!");
+
+    return true;
+  }
+
+  /**
+     * @brief Formats the memory.
+     * Use this only if the filesystem is in a good state: this fuction
+     * reads every sector header and erases only sector where
+     * header content != SECTOR_EMPTY
+     *
+     * @warning Use this only for testing!
+     */
+  static bool fastFormat()
+  {
+    logger.warning(logtag(), "!!!THE MEMORY IS ABOUT TO BE FORMATTED!!!\n");
+
+    logger.info(logtag(),"Formatting...\n");
+    //TODO: Flash leds for a few seconds as a warning
+    //Always erase the first sector
+    if(!erase(0, SUBSECTORS_PER_SECTOR)){
+      logger.error(logtag(), "Couldn't erase sector 0");
+      return false;
+    }
+
+    //Then erase only the written/broken sectors
+    uint32_t s;
+    for (s = 1; s < SECTORS_NUM; s++)
+    {
+      SectorHeader header;
+      bool read_result = readSectorHeader(s, &header);
+      if (read_result && header.content != SECTOR_EMPTY)
+      {
+        //Erase data in this sector
+        if(!erase(s * SUBSECTORS_PER_SECTOR, (s + 1) * SUBSECTORS_PER_SECTOR)){
+          logger.error(logtag(), "Couldn't erase sector ", s);
+          return false;
+        }
+
+        //Rewrite the header
+        SectorHeader sheader;
+        sheader.content = SECTOR_EMPTY;
+        if (!writeSectorHeader(s, sheader))
+        {
+          logger.error("Error writing sector header in sector ", s);
+          return false;
+        }
+      }else if(!read_result)
+      {
+        logger.error(logtag(), "Error reading sector header in sector ", s);
+        return false;
+      }
+    }
+
+    //Format the memory
+    FlashHeader header { };
+    header.first_boot = FIRST_BOOT_KEY;
+
+    if(!writeFlashHeader(header, false)){
+      logger.error(logtag(),"Couldn't write flash header.");
+      return false;
+    }
+
+    return true;
+  }
+
+  static bool eraseMemory(uint8_t* result)
+  {
+    return erase(0, SUBSECTORS_NUM);
+  }
+
+  /**
+   * Erases the memory between the given subsectors
+   * Only erases a subsector if data has been written into it,
+   * to avoid wasting erase cycles.
+   *
+   * @brief Erases The memory between the given bounds.
+   * @param result Erase successful or not
+   * @param from Address belonging to the first subsector to be erased
+   * @param to Address belonging to the last subsector to be erased
+   * @return
+   */
+  static bool erase(uint32_t from_subsector, uint32_t to_subsector)
+  {
+    uint8_t result;
+    FlashDriverType* flash = Singleton<FlashDriverType>::getInstance();
+    uint8_t* buffer = new uint8_t[PAGE_SIZE];
+
+    std::stringstream stream;  //To save strings to write in a single log line
+    stream << "Erasing subsectors: ";
+
+    //Erase from the start of the subsector corresponding to "from"
+    uint32_t from_addr = from_subsector * SUBSECTOR_SIZE;
+
+    //To the end of the subsector corresponding to "to".
+    uint32_t to_addr = to_subsector * SUBSECTOR_SIZE;
+
+    //Read memory in blocks of PAGE_SIZE, if a byte is != 0xFF, erase the
+    //subsector and start reading again from the next one.
+    while (from_addr < to_addr)
+    {
+      flash->read(&result, from_addr, buffer, PAGE_SIZE);
+
+      if (result == OpResultFlags::RESULT_OK)
+      {
+        bool erase = false;
+        for (uint32_t i = 0; i < PAGE_SIZE; i++)
+        {
+          erase = buffer[i] != 0xFF;
+
+          if (erase)
+            break;
+        }
+
+        if (erase)
+        {
+          stream << from_addr / SUBSECTOR_SIZE << ", ";
+
+          flash->enableErase();
+          flash->eraseSubsector(&result, from_addr);
+          if (result == OpResultFlags::RESULT_OK)
+          {
+            //Move to the beginning of the next subsector
+            from_addr = (from_addr / SUBSECTOR_SIZE + 1) * SUBSECTOR_SIZE;
+          } else
+          {
+            logger.error(logtag(), "Error erasing subsector: result=0x",
+                         logging::hex(result));
+            break;
+          }
+
+        } else
+        {
+          from_addr = from_addr + PAGE_SIZE;
+        }
+      } else
+      {
+        logger.error(logtag(), "Error reading memory: result=0x",
+                     logging::hex(result));
+        break;
+      }
+    }
+
+    stream << "--";
+
+    logger.info(logtag(), stream.str());
+    delete buffer;
+
+    return from_addr >= to_addr;
+  }
+
+  static bool isMemoryClean(uint8_t* result)
+  {
+    FlashDriverType* flash = Singleton<FlashDriverType>::getInstance();
+    uint8_t buffer[BLOCK_SIZE];
+
+    //Check from the start of the subsector corresponding to "from"
+    uint32_t addr = SUBSECTOR_SIZE;
+
+    bool clean = true;
+    while (addr < MEMORY_SIZE)
+    {
+      if (addr % SECTOR_SIZE == 0)
+      {
+        SectorHeader header;
+        bool read_result = readSectorHeader(addr / SECTOR_SIZE, &header);
+        if (!read_result || header.content != SECTOR_EMPTY)
+        {
+          logger.warning(logtag(), "Wrong sector header @", addr);
+          clean = false;
+        }
+      } else
+      {
+        flash->read(result, addr, buffer, BLOCK_SIZE);
+        if (*result == OpResultFlags::RESULT_OK)
+        {
+          for (uint32_t i = 0; i < BLOCK_SIZE; i++)
+          {
+            if (buffer[i] != 0xFF)
+            {
+              logger.warning(logtag(), "Dirty block @", addr);
+              clean = false;
+              break;
+            }
+          }
+        } else
+        {
+          logger.warning(logtag(), "Error reading at %d\n", addr);
+          break;
+        }
+      }
+
+      addr = addr + BLOCK_SIZE;
+    }
+
+    if (clean)
+    {
+      logger.info(logtag(), "Memory is clean.");
+    }
+    return clean;
+  }
+
+ private:
+
+  FlashController()
+  {
+    flash_ = Singleton<FlashDriverType>::getInstance();
+  }
+
+  /**
+   * @brief Restores the state after a reboot.
+   *
+   * Restores the state after a reboot.
+   * Sequentially search for the first empty sector and update block_index_
+   * accordingly
+   *
+   * @return Empty sector found or not
+   */
+  bool recoverFromReboot()
+  {
+    //Start from the second sector
+    uint32_t sector = 1;
+    SectorHeader header;
+
+    do
+    {
+      bool r = readSectorHeader(sector, &header);
+      if (r && header.content == SECTOR_EMPTY)
+      {
+        block_index_ = sector * BLOCKS_PER_SECTOR;
+        return true;
+      }
+    } while (++sector < SECTORS_NUM);
+
+    return false;
+  }
+
+  /**
+   * Updates block_index_ to point to the next available sector.
+   *
+   * @return True if a new sector is found
+   */
+  bool gotoNewSector()
+  {
+    uint32_t sector = block_index_ / BLOCKS_PER_SECTOR;
+    SectorHeader readHeader, writeHeader;
+
+    //Look for an empty sector
+    bool read_result;
+    int i = 0;
+    do
+    {
+      read_result = readSectorHeader(sector, &readHeader);
+
+    } while (((read_result && readHeader.content != SECTOR_EMPTY) || !read_result)
+        && ++sector < SECTORS_NUM && ++i < MAX_SECTOR_ITERATIONS);
+
+    block_index_ = sector * BLOCKS_PER_SECTOR;
+
+    if (read_result && readHeader.content == SECTOR_EMPTY)
+    {
+      writeHeader.content = SECTOR_WRITTEN;
+
+      if (!writeSectorHeader(sector, writeHeader))
+      {
+        logger.error(logtag(), "Error writing sector header.");
+        return false;
+      }
+      block_index_++;
+    } else
+    {
+      logger.error(logtag(), "Error: could not find empty sector to write in.");
+      return false;
+    }
+
+    return true;
+  }
+
+  static std::string logtag()
+  {
+    return "FlashCTRL";
+  }
+
+  FlashDriverType* flash_;
+  uint32_t block_index_ = 0;
+  //Value will be changed if initialization is successful
+  Status status_ = Status::INITIALIZATION_ERROR;
+
+};
+
+}   //namespace flashmemory
+
+#endif /* FLASHCONTROLLER_H */
diff --git a/src/shared/drivers/memory/FlashDriver.h b/src/shared/drivers/memory/FlashDriver.h
new file mode 100644
index 0000000000000000000000000000000000000000..2f4f5f70a4a0ecdd540a8851514ebdfcd14b059d
--- /dev/null
+++ b/src/shared/drivers/memory/FlashDriver.h
@@ -0,0 +1,595 @@
+
+
+
+#ifndef FLASHDRIVER_H
+#define FLASHDRIVER_H
+
+#include <miosix.h>
+#include <Singleton.h>
+#include <diagnostic/NewLogger.h>
+#include <string>
+
+using miosix::Thread;
+using miosix::Mode;
+
+using logging::logger;
+
+namespace flashmemory {
+
+static const uint32_t PAGES_PER_SUBSECTOR = 16U;
+static const uint32_t PAGES_PER_SECTOR = 256U;
+static const uint32_t SUBSECTORS_PER_SECTOR = 16U;
+
+static const uint32_t SUBSECTORS_NUM = 16384U;
+static const uint32_t SECTORS_NUM = 1024U;
+static const uint32_t PAGE_SIZE = 256U;
+
+static const uint32_t SUBSECTOR_SIZE = PAGE_SIZE*PAGES_PER_SUBSECTOR;
+static const uint32_t SECTOR_SIZE = PAGE_SIZE*PAGES_PER_SECTOR;
+
+static const uint32_t BANK_SIZE = SECTOR_SIZE*SECTORS_NUM/2;
+static const uint32_t MEMORY_SIZE = SECTOR_SIZE*SECTORS_NUM;
+
+
+/**
+ * 	@brief Definitions for results of various flash operations.
+ *
+ * 	Definitions for result flags of various flash operations.
+ */
+enum OpResultFlags
+{
+	RESULT_OK 				= 0x00,
+
+
+	RESULT_F_OUT_OF_MEMORY 	= 0x01,
+	RESULT_F_CHECK_FAIL 		= 0x04,
+
+
+	//Bits reserved for future use
+	RESULT_F_UNUSED_1			= 0x08,
+	RESULT_F_UNUSED_2 		= 0x40,
+	RESULT_F_UNUSED_3 		= 0x80,
+
+
+	/**
+	 * These bits are in the same positions of the corresponding bits in the
+	 * FLAG STATUS REGISTER
+	 */
+	RESULT_F_PROTECTION_ERROR = 0x02,
+	RESULT_F_PROGRAM_ERROR 	= 0x10,
+	RESULT_F_ERASE_ERROR		= 0x20
+};
+
+
+/*
+ * TODO:
+ * -Controlla significato bit Vpp nel Flag status register
+ * -Casi in cui eseguire _write_disable()
+ * -Velocizzare il clock software per il factory reset
+ * -Controlla se bisogna davvero leggere due volte il flag_status_register per
+ * 	vedere se una scrittura ad un registro e' stata completata
+ *
+ */
+
+template<typename Bus>
+class FlashDriver : Singleton<FlashDriver<Bus>>
+{
+  typedef Singleton<FlashDriver<Bus>> SingletonType;
+
+  friend class Singleton<FlashDriver<Bus>>;
+
+	template<typename>
+	friend class FlashDriverTest;
+
+public:
+
+	/**
+	 * @brief Read n bytes into a buffer, starting from the specified address
+	 *
+	 * @param result
+	 * @param address Starting address
+	 * @param buf Buffer to read data into
+	 * @param size Number of bytes to read
+	 */
+	void read(uint8_t *result, uint32_t address, uint8_t* buf, uint32_t size)
+	{
+		if(address + size > MEMORY_SIZE){
+			*result = RESULT_F_OUT_OF_MEMORY;
+			return;
+		}
+
+		uint32_t rsize;
+		if(address < BANK_SIZE)
+			rsize = std::min(size, BANK_SIZE - address);
+		else
+			rsize = size;
+
+		uint8_t addr_buf[4];
+		addrToBuf(addr_buf, address);
+
+		Bus::read(READ, addr_buf, buf, 4, rsize);
+
+		size -= rsize;
+
+		//This means that we reached the end of the first die but we still need
+		//to read some data.
+		if(size > 0)
+		{
+			address += rsize;
+			buf += rsize;
+
+			addrToBuf(addr_buf, address);
+
+			//Read the remaining bytes
+			Bus::read(READ, addr_buf, buf, 4, size);
+		}
+
+		*result = RESULT_OK;
+	}
+
+	/**
+	 * @brief Writes data on the flash starting from the specified address.
+	 *
+	 * @param address
+	 * @param data
+	 * @param size
+	 */
+	void write(uint8_t *result, uint32_t address, uint8_t* data, uint32_t size)
+	{
+		//Do not write outside of the memory
+		if(address + size > MEMORY_SIZE)
+		{
+			*result = RESULT_F_OUT_OF_MEMORY;
+			return;
+		}
+
+		uint8_t addr_buf[4], status;
+
+		uint32_t next_page, wsize;
+
+		do {
+			next_page = (address / 256)*256 + 256;
+
+			wsize = std::min(next_page - address, size);
+
+			addrToBuf(addr_buf, address);
+			writeEnable();
+			Bus::write(PROGRAM_PAGE, addr_buf, data, 4, wsize);
+
+			do {
+				status = readFlagStatusReg();
+			}while((status & FSR_PRG_ERS_CTRL) == 0);
+
+			data += wsize;
+			size -= wsize;
+			address = next_page;
+
+			*result = status & (FSR_PROGRAM | FSR_PROTECTION);
+		}while(size > 0 && *result == RESULT_OK);
+
+		//Clear flag status register if an error occurred
+		if(*result != RESULT_OK)
+		{
+			clearFlagStatusReg();
+
+			//Manually reset write_enable latch
+			if((*result & RESULT_F_PROTECTION_ERROR) > 0)
+			{
+				writeDisable();
+			}
+		}
+	}
+
+	/**
+	 * @brief Writes data on the flash starting from the specified address, then
+	 * checks if everything was written correctly.
+	 *
+	 * @param address
+	 * @param data
+	 * @param size
+	 */
+	void writeAndCheck(uint8_t *result, uint32_t address, uint8_t* data,
+			uint32_t size){ //TODO: size_t
+		write(result,address,data,size);
+		if(*result == RESULT_OK){
+			uint8_t *check = new uint8_t[size];
+			read(result, address, check, size);
+			for(uint32_t i = 0; i < size; i++){
+				if(*check++ != *data++){
+					//Something was not written/read correctly.
+					*result = RESULT_F_CHECK_FAIL;
+					break;
+				}
+			}
+		}
+	}
+
+	/**
+	 * @brief Writes data on the flash starting from the specified address, only
+	 * if the provided data fits in a single page.
+	 *
+	 * Writes data on the flash starting from the specified address, only
+	 * if the provided data fits in a single page. If too much data is provided
+	 * nothing will be written and result will be set to RESULT_F_OUT_OF_MEMORY
+	 *
+	 * @param result
+	 * @param address
+	 * @param data
+	 * @param size
+	 */
+	void programPage(uint8_t *result, uint32_t address, uint8_t* data, uint32_t size)
+	{
+		//Do now wrap around a page and do not try to write outside of the memory
+		uint32_t next_page = (address / 256)*256 + 256;
+		if(address + size > next_page || address > MEMORY_SIZE)
+		{
+			*result = RESULT_F_OUT_OF_MEMORY;
+			return;
+		}
+
+		uint8_t addr_buf[4], status;
+		addrToBuf(addr_buf, address);
+
+		writeEnable();
+		Bus::write(PROGRAM_PAGE, addr_buf, data, 4, size);
+
+		do {
+			status = readFlagStatusReg();
+		}while((status & FSR_PRG_ERS_CTRL) == 0);
+
+		*result = status & (FSR_PROGRAM | FSR_PROTECTION);
+
+		//Clear flag status register if an error has been encountered
+		if(*result != RESULT_OK)
+		{
+			clearFlagStatusReg();
+
+			//Manually reset write_enable latch
+			if((*result & RESULT_F_PROTECTION_ERROR) > 0)
+			{
+				writeDisable();
+			}
+		}
+	}
+
+	/**
+	 * @brief Enables or disables read only mode
+	 */
+	void setReadOnly(bool readonly = true)
+	{
+		read_only_ = readonly;
+	}
+
+
+	bool isReadOnly() const
+	{
+	  return read_only_;
+	}
+
+	/**
+	 * @brief Enable the next erase operation. Call this before every erase op.
+	 */
+	void enableErase()
+	{
+		writeEnable();
+	}
+
+	/**
+	 * @brief Erases the subsector containing the specified address. For the operation
+	 * to be successful, enable_erase_op must be called before this.
+	 *
+	 * @warning Estimated execution time is slightly less than 1 second.
+	 *
+	 * @param result
+	 * @param address
+	 */
+	void eraseSubsector(uint8_t* result, uint32_t address)
+	{
+		erase(result, SUBSECTOR_ERASE, address);
+	}
+
+	/**
+	 * @brief Erases the sector containing the specified address. For the operation
+	 * to be successful, enable_erase_op must be called before this.
+	 * @warning Estimated execution time is about 2 seconds.
+	 *
+	 * @param result
+	 * @param address
+	 */
+	void eraseSector(uint8_t* result, uint32_t address)
+	{
+		erase(result, SECTOR_ERASE, address);
+	}
+
+	/**
+	 * @brief Erases the entire die containing the specified address. For the operation
+	 * to be successful, enable_erase_op must be called before this.
+	 * @warning Estimated execution time is more than 4 minutes.
+	 *
+	 * @param result
+	 * @param die: 0 to erase the first die, 1 to erase the second.
+	 */
+	void eraseDie(uint8_t* result, uint8_t die)
+	{
+		if(die == 0){
+			erase(result, DIE_ERASE, 0);
+		}else if(die == 1){
+			erase(result, DIE_ERASE, BANK_SIZE);
+		}
+	}
+
+	/**
+	 * Reads id information for this flash memory.
+	 * @param buf Buffer with at least 20 bytes
+	 */
+	void readId(uint8_t* buf)
+	{
+		Bus::read(READ_ID, buf, 20);
+	}
+
+
+	/**
+	 * @brief Software reset for the memory. All volatile bits are set to their
+	 * default value.
+	 */
+	void softReset()
+	{
+		Bus::write(RESET_ENABLE);
+		usleep(5);
+		Bus::write(RESET_MEMORY);
+		waitUntilReady();
+	}
+
+	/**
+	 * If the flash memory is in a bad state and can't be recovered by any
+	 * other means, run this, then set CONFIG_REGISTER to 0xFFFF.
+	 */
+	template <class CS, class MOSI, class CLK>
+	static void factoryReset()
+	{
+		{
+			miosix::FastInterruptDisableLock dLock;
+			CS::mode(Mode::OUTPUT);
+			MOSI::mode(Mode::OUTPUT);
+			CLK::mode(Mode::OUTPUT);
+		}
+		MOSI::high();
+		Thread::sleep(20);
+
+
+		factoryResetSequence<CS, CLK>(7);
+		factoryResetSequence<CS, CLK>(13);
+		factoryResetSequence<CS, CLK>(17);
+		factoryResetSequence<CS, CLK>(25);
+		factoryResetSequence<CS, CLK>(33);
+
+		MOSI::low();
+		Thread::sleep(50);
+		MOSI::high();
+		factoryResetSequence<CS, CLK>(8);
+
+		MOSI::low();
+		CS::low();
+	}
+
+private:
+	FlashDriver(){
+	  waitUntilReady();
+
+    uint8_t buf[20];
+    readId(buf);
+
+    if(buf[0] != 0x20)
+    {
+      logger.critical(logtag(), "Wrong id on FlashDriver instantiation");
+    }
+
+    writeStatusReg(0x00);
+
+    clearFlagStatusReg();
+
+
+    uint16_t config = readConfigReg();
+
+    //Last bit to 0 to enable 4 byte address mode
+    if(config != 0xFFFE) writeConfigReg(0xFFFE);
+	}
+
+	void erase(uint8_t *result, uint8_t erase_cmd, uint32_t address)
+	{
+		if(address > MEMORY_SIZE)
+		{
+			*result = RESULT_F_OUT_OF_MEMORY;
+			return;
+		}
+
+		uint8_t addr_buf[4], status;
+		addrToBuf(addr_buf, address);
+		Bus::write(erase_cmd, addr_buf, 4);
+
+		do {
+			status = readFlagStatusReg();
+		}while((status & FSR_PRG_ERS_CTRL) == 0);
+
+		*result = status & (FSR_ERASE | FSR_PROTECTION);
+
+		//Clear flag status register if an error has been encountered
+		if(*result != RESULT_OK)
+		{
+			clearFlagStatusReg();
+
+			//Manually reset write_enable latch
+			if((*result & RESULT_F_PROTECTION_ERROR) > 0)
+			{
+				writeDisable();
+			}
+		}
+	}
+
+	/**
+   * @brief Checks if a program operation is still in progress.
+   * @param result
+   */
+  bool isBusy()
+  {
+    return readFlagStatusReg() & FSR_PRG_ERS_CTRL;
+  }
+
+  /**
+   * @brief Waits until the memory is ready to perform a new write/erase op.
+   * @param result
+   */
+  void waitUntilReady()
+  {
+    while((readFlagStatusReg() & FSR_PRG_ERS_CTRL) == 0)
+    {
+
+    }
+  }
+
+	uint8_t readStatusReg()
+	{
+		uint8_t ret = Bus::read(READ_STATUS_REG);
+		return ret;
+	}
+
+	void writeStatusReg(uint8_t val)
+	{
+		writeEnable();
+		Bus::write(WRITE_STATUS_REG, val);
+		writeDisable();
+		waitUntilReady();
+	}
+
+	uint8_t readFlagStatusReg()
+	{
+		uint8_t ret = Bus::read(READ_FLAG_STATUS_REG);
+		return ret;
+	}
+
+	void clearFlagStatusReg()
+	{
+		Bus::write(CLEAR_FLAG_STATUS_REG);
+	}
+
+	uint16_t readConfigReg()
+	{
+		uint8_t buf[2];
+		Bus::read(READ_NV_CONFIG_REG, buf, 2);
+
+		uint16_t res = buf[0] | (buf[1] << 8);
+
+		return res;
+	}
+
+	void writeConfigReg(uint16_t val)
+	{
+		/*
+		 * Mask some of the bits of the config register because
+		 * we don't want to change them by accident.
+		 */
+		val = val | 0x0FEC;
+
+		uint8_t buf[2];
+		buf[0] = val & 0xFF;
+		buf[1] = (val >> 8) & 0xFF;
+
+		writeEnable();
+		Bus::write(WRITE_NV_CONFIG_REG, buf, 2);
+		writeDisable();
+		waitUntilReady();
+	}
+
+	void writeEnable()
+	{
+		if(!read_only_) {
+			Bus::write(WRITE_ENABLE);
+		}
+	}
+
+	void writeDisable()
+	{
+		Bus::write(WRITE_DISABLE);
+	}
+
+	static void addrToBuf(uint8_t* buf, uint32_t addr)
+	{
+		for(int i = 0; i < 4; i++)
+		{
+			buf[3 - i] = (addr >> 8*i) & 0xFF;
+		}
+	}
+
+	template <class CS, class CLK>
+	static void factoryResetSequence(int cycles)
+	{
+		CS::low();
+		Thread::sleep(5);
+		for(int i = 0; i < cycles; i++)
+		{
+			CLK::high();
+			Thread::sleep(5);
+			CLK::low();
+			Thread::sleep(5);
+		}
+		CS::high();
+		Thread::sleep(20);
+	}
+
+	static std::string logtag()
+	{
+	  return "FLASH_DRIVER";
+	}
+
+	enum Commands
+	{
+		READ_ID = 0x9F,
+
+		WRITE_ENABLE = 0x06,
+		WRITE_DISABLE = 0x04,
+
+		READ_STATUS_REG = 0x05,
+		WRITE_STATUS_REG = 0x01,
+
+		READ_FLAG_STATUS_REG = 0x70,
+		CLEAR_FLAG_STATUS_REG = 0x50,
+
+		//non-volatile configuration register
+		READ_NV_CONFIG_REG = 0xB5,
+		WRITE_NV_CONFIG_REG = 0xB1,
+
+		//volatile configuration register
+		WRITE_VOL_CONFIG_REG = 0x85,
+		READ_VOL_CONFIG_REG = 0x81,
+
+		READ_EXT_ADDRESS_REG = 0xC8,
+		WRITE_EXT_ADDRESS_REG = 0xC5,
+
+		RESET_ENABLE = 0x66,
+		RESET_MEMORY = 0x99,
+
+		READ = 0x03,
+		PROGRAM_PAGE = 0x02,
+
+		SUBSECTOR_ERASE = 0x20,
+		SECTOR_ERASE = 0xD8,
+		DIE_ERASE = 0xC4
+	};
+
+	bool read_only_ = false;
+
+	/**
+	 * Flag status register bit definitions
+	 */
+	static const uint8_t FSR_ADDRESSING_MODE		= 0x01;
+	static const uint8_t FSR_PROTECTION 			= 0x02;
+	static const uint8_t FSR_PROGRAM_SUSPEND 		= 0x04;
+	static const uint8_t FSR_VPP 					= 0x08;
+	static const uint8_t FSR_PROGRAM 				= 0x10;
+	static const uint8_t FSR_ERASE 					= 0x20;
+	static const uint8_t FSR_ERASE_SUSPEND			= 0x40;
+	static const uint8_t FSR_PRG_ERS_CTRL	 		= 0x80;
+};
+
+}
+#endif /* FLASHDRIVER_H */
diff --git a/src/shared/drivers/memory/FlashDriverInclude.h b/src/shared/drivers/memory/FlashDriverInclude.h
new file mode 100644
index 0000000000000000000000000000000000000000..30e28b85707f6eaa0248c42bedc2f1d5bf5edda6
--- /dev/null
+++ b/src/shared/drivers/memory/FlashDriverInclude.h
@@ -0,0 +1,38 @@
+/* Copyright (c) 2015-2017 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.
+ */
+
+#ifndef SRC_SHARED_DRIVERS_MEMORY_FLASHDRIVERINCLUDE_H_
+#define SRC_SHARED_DRIVERS_MEMORY_FLASHDRIVERINCLUDE_H_
+
+
+#ifdef FLASH_TEST
+
+#include <flashmemory/mockup/FlashDriver.h>
+
+#else
+
+#include <drivers/memory/FlashDriver.h>
+
+#endif /* FLASH_TEST */
+
+
+#endif /* SRC_SHARED_DRIVERS_MEMORY_FLASHDRIVERINCLUDE_H_ */
diff --git a/src/shared/drivers/memory/MultiFlashController.cpp b/src/shared/drivers/memory/MultiFlashController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ae0aeef1623ca1874f930f2fd86529d94a7bf9a7
--- /dev/null
+++ b/src/shared/drivers/memory/MultiFlashController.cpp
@@ -0,0 +1,91 @@
+/*
+ * MultiFlashController.cpp
+ *
+ *  Created on: Sep 17, 2017
+ *      Author: luca
+ */
+
+#include "MultiFlashController.h"
+#include <Singleton.h>
+#include <diagnostic/NewLogger.h>
+
+using logging::logger;
+/* stub crc class */
+
+namespace flashmemory
+{
+class CRCHelper
+{
+ public:
+  static uint16_t calcCRC16(uint8_t* data, uint32_t size)
+  {
+    return 0xABCD;
+  }
+};
+
+//miosix::Queue<FlashDataBlock,20> MultiFlashController::block_queue_;
+
+MultiFlashController::MultiFlashController()
+    : ActiveObject::ActiveObject()
+{
+  flash0_ = Singleton<FlashController<MemoryBus0>>::getInstance();
+
+  //block_queue_ = new miosix::Queue<FlashDataBlock, 5>();
+}
+
+MultiFlashController::~MultiFlashController()
+{
+}
+
+/**
+ *   @brief  Infinite loop that process the block queue.
+ */
+void MultiFlashController::run()
+{
+  FlashDataBlock to_flash;
+  while (true)
+  {
+    block_queue_.waitUntilNotEmpty();
+
+    block_queue_.get(to_flash);
+
+    bool result = true;
+
+    result = result && flash0_->writeDataBlock(&to_flash);
+
+    //TODO: Also write on 2nd and 3rd flash memory
+    //result = result && flash1_->writeBlock(&to_flash);
+    //result = result && flash1_->writeBlock(&to_flash);
+
+    if (!result)
+    {
+      logger.error(logtag(), "Error writing blocks!");
+    }
+
+  }
+}
+
+/**
+ *   @brief  Insert into the fixed queue a new FlashDataBlock to save in flash.
+ *
+ *   @param  buffer data do be saved
+ *   @param  size the size of the data (must be < FLASH_BUFFER_SIZE)
+ */
+void MultiFlashController::addData(const uint8_t* buffer, size_t size)
+{
+  /* limit data to FLASH_BUFFER_SIZE */
+  uint16_t safe_size = (size <= FLASH_BUFFER_SIZE ? size : FLASH_BUFFER_SIZE);
+  FlashDataBlock to_save = { };
+
+  // create the Flash Data Block.
+  to_save.id = block_counter++;
+  to_save.timestamp = miosix::getTick();
+  memcpy((uint8_t*) to_save.buffer, (uint8_t*) buffer, safe_size);
+
+  //TODO crc of the whole block with toSave.crc = 0x0000?
+  to_save.crc = CRCHelper::calcCRC16((uint8_t*) to_save.buffer, safe_size);
+
+  block_queue_.put(to_save);
+}
+
+}  //namespace flashmemory
diff --git a/src/shared/drivers/memory/MultiFlashController.h b/src/shared/drivers/memory/MultiFlashController.h
new file mode 100644
index 0000000000000000000000000000000000000000..9f1d42e0df98ad6e9ab780a1a49e50e7ab9270ca
--- /dev/null
+++ b/src/shared/drivers/memory/MultiFlashController.h
@@ -0,0 +1,73 @@
+/*
+ * MultiFlashController.h
+ *
+ *  Created on: Sep 13, 2017
+ *      Author: luca
+ */
+#ifndef MULTIFLASHCONTROLLER_H
+#define MULTIFLASHCONTROLLER_H
+
+#include <Common.h>
+#include <ActiveObject.h>
+#include <drivers/BusTemplate.h>
+#include "FlashController.h"
+
+using miosix::Gpio;
+
+//Forward declaration
+namespace testing
+{
+namespace flashmemorytests
+{
+template<typename >
+class FlashTest;
+}
+}
+
+namespace flashmemory
+{
+
+class MultiFlashController : public Singleton<MultiFlashController>, ActiveObject
+{
+  friend class Singleton<MultiFlashController>;
+
+  template<typename >
+  friend class testing::flashmemorytests::FlashTest;
+
+  typedef Gpio<GPIOA_BASE, 5> GpioSck;
+  typedef Gpio<GPIOA_BASE, 6> GpioMiso;
+  typedef Gpio<GPIOA_BASE, 7> GpioMosi;
+  typedef BusSPI<1, GpioMosi, GpioMiso, GpioSck> bus;
+
+  typedef Gpio<GPIOC_BASE, 15> CS_FLASH0;
+
+ public:
+  typedef ProtocolSPI<bus, CS_FLASH0> MemoryBus0;
+
+  virtual ~MultiFlashController();
+
+  void addData(const uint8_t* buffer, size_t size);
+
+ protected:
+
+  void run() override;
+
+ private:
+  MultiFlashController();
+
+  static std::string logtag()
+  {
+    return "MultiFlashCTRL";
+  }
+
+  uint32_t block_counter = 0;
+  miosix::Queue<FlashDataBlock,20> block_queue_;
+
+  FlashController<MemoryBus0>* flash0_;
+  //FlashController<spi_flash0> flash1; //Change to spi_flash1
+  //FlashController<spi_flash0> flash2; //Change to spi_flash2
+};
+
+}  //namespace flashmemory
+
+#endif /* MULTIFLASHCONTROLLER_H */
diff --git a/src/shared/sensors/LSM6DS3H.h b/src/shared/sensors/LSM6DS3H.h
deleted file mode 100644
index fbc35c321bde7a94201d8d728438d3d7ace4144b..0000000000000000000000000000000000000000
--- a/src/shared/sensors/LSM6DS3H.h
+++ /dev/null
@@ -1,423 +0,0 @@
-/*  LSM6DS3H Driver
- *
- * Copyright (c) 2018-2019 Skyward Experimental Rocketry
- * Authors: Soufiane Machkouk
- *
- * 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 <drivers/BusTemplate.h>
-#include "Common.h"
-#include "Sensor.h"
-#include "miosix.h"
-template <typename BusG>
-class LSM6DS3H : public GyroSensor, public AccelSensor
-{
-public:
-    LSM6DS3H(uint8_t accelFullScale, uint16_t gyroFullScale)
-    {
-        accelFS = accelFullScale;
-        gyroFS  = gyroFullScale;
-    }
-    bool init()
-    {
-        selfTest();
-        BusG::write(RegMap::CTRL5_C, 0x60);  //  Rounding read enabled for both accel and gyro
-        BusG::write(RegMap::CTRL3_C, 0x44 ); // incrementare l'indirizzo ad ogni lettura data LSB @ lower address
-        fifoConfiguration();
-        accelConfiguration();
-        gyroConfiguration();
-        // TODO
-        uint8_t XLSamplesToDiscard=2;   //
-        uint8_t gyroSamplesToDiscard=2; //since we set ODR to 416HZ
-        uint8_t buffer[2]={0, 0};
-        BusG::read(RegMap::FIFO_DATA_OUT_L, buffer, XLSamplesToDiscard);
-        BusG::read(RegMap::FIFO_DATA_OUT_L, buffer, gyroSamplesToDiscard);
-        return true;
-    }
-    void fifoConfiguration()
-    {
-        uint8_t continuousMode = 0x36;  // ODR is set to 416HZ
-        BusG::write(RegMap::FIFO_CTRL_REG5, continuousMode);
-        uint8_t bitToSet= BusG::read(RegMap::FIFO_CTRL_REG2);
-        bitToSet = bitToSet & 0xcf;
-        BusG::write(RegMap::FIFO_CTRL_REG2, bitToSet);
-        uint8_t noDecimation = 0x09;
-        BusG::write(RegMap::FIFO_CTRL_REG3, noDecimation);  // NO decimation for both accel and gyro data
-        bitToSet = 0x00;
-        BusG::write(RegMap::FIFO_CTRL_REG5, bitToSet);
-        uint8_t registerStatus = BusG::read(RegMap::FIFO_STATUS4);
-        bitToSet = 0x03;
-        bitToSet = bitToSet & registerStatus;
-        BusG::write(RegMap::FIFO_STATUS4, bitToSet);
-    }
-    bool selfTest()
-    {
-        uint8_t whoami = BusG::read(RegMap::WHO_AM_I);
-        if (whoami != whoami_value)
-        {
-            last_error = ERR_NOT_ME;
-            return false;
-        }
-        return (accelSelfTest() & gyroSelfTest());
-    }
-    void accelConfiguration()
-    {
-        uint8_t i=0;
-        uint8_t controlAccel = 0x61; 
-        for (i = 0; i<=3; i++)
-        {
-            if ((uint8_t)accelFSMAP[i] == accelFS)
-            {
-                controlAccel = controlAccel | (i << 2);
-                BusG::write(RegMap::CTRL1_XL, controlAccel);  // anti-aliasing filter set to 200Hz
-                                            // ODR to 416HZ
-            }
-        }
-
-        BusG::write(RegMap::CTRL8_XL, 0x00); // no LPF2
-        uint8_t enableXYZ = 0x38;
-        BusG::write(RegMap::CTRL9_XL, enableXYZ);
-    }
-    void gyroConfiguration()
-    {
-        uint8_t i=0;
-        uint8_t controlGyro = 0x60;
-        for(i=0; i<=3; i++)
-        {
-            if ((uint16_t)gyroFSMAP[i] == gyroFS)
-            {
-                controlGyro = controlGyro | (i << 2);
-                BusG::write(RegMap::CTRL2_G, controlGyro);
-            }
-        }
-        uint8_t enableXYZ = 0x39;
-        BusG::write(RegMap::CTRL10_C, enableXYZ);
-        //HPfilter disabled
-    }
-    bool onSimpleUpdate() { return false; }
-    void sampleUpdate()
-    {
-        //gyro data set ---> lettura da 6 byte
-        BusG::read(FIFO_DATA_OUT_L, datiGyro, 6);
-        // accel dataset ---> lettura da 6 byte
-        BusG::read(FIFO_DATA_OUT_L, datiAccel, 6);
-        Data.gyroXSample   = toFloat(datiGyro[0]+datiGyro[1]<<8, gyroFS);
-        Data.gyroYSample   = toFloat(datiGyro[2]+datiGyro[3]<<8, gyroFS);
-        Data.gyroZSample   = toFloat(datiGyro[4]+datiGyro[5]<<8, gyroFS);
-        Data.accelXSample  = toFloat(datiAccel[0]+datiAccel[1]<<8, accelFS);
-        Data.accelYSample  = toFloat(datiAccel[2]+datiAccel[3]<<8, accelFS);
-        Data.accelZSample  = toFloat(datiAccel[4]+datiAccel[5]<<8, accelFS);
-       
-    }
-   
-    bool accelSelfTest(){
-        uint8_t discard;
-        uint8_t c=0;
-        uint8_t rxData[2];
-        float OUTX_NOST[]={0, 0, 0, 0, 0};
-        float outXNoST;
-        float OUTY_NOST[]={0, 0, 0, 0, 0};
-        float outYNoST;
-        float OUTZ_NOST[]={0, 0, 0, 0, 0};
-        float outZNoST;
-        float OUTX_ST[]={0, 0, 0, 0, 0};
-        float outXST;
-        float OUTY_ST[]={0, 0, 0, 0, 0};
-        float outYST;
-        float OUTZ_ST[]={0, 0, 0, 0, 0};
-        float outZST;
-        BusG::write(CTRL1_XL, 0x30);
-        BusG::write(CTRL2_G, 0x00);
-        BusG::write(CTRL3_C, 0x44);
-        BusG::write(CTRL4_C, 0x00);
-        BusG::write(CTRL5_C, 0x00);
-        BusG::write(CTRL6_C, 0x00);
-        BusG::write(CTRL7_G, 0x00);
-        BusG::write(CTRL8_XL, 0x00);
-        BusG::write(CTRL9_XL, 0x38);
-        BusG::write(CTRL10_C, 0x00);
-        miosix::Thread::sleep(200);
-        discard = BusG::read(STATUS_REG);
-        while ((discard & 0x01) == 0 ){
-            discard = BusG::read(STATUS_REG);
-        }
-        BusG::read(OUT_X_L_XL, rxData, 2);
-        BusG::read(OUT_Y_L_XL, rxData, 2);
-        BusG::read(OUT_Z_L_XL, rxData, 2);
-        discard = BusG::read(STATUS_REG);
-        while ( c<=5 ){
-            while ((discard & 0x01) == 0 ){
-                discard = BusG::read(STATUS_REG);
-            }
-            BusG::read(OUT_X_L_XL, rxData, 2);
-            OUTX_NOST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2); 
-            BusG::read(OUT_Y_L_XL, rxData, 2);
-            OUTY_NOST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2);
-            BusG::read(OUT_Z_L_XL, rxData, 2);
-            OUTZ_NOST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2);
-            c++;
-        }
-        outXNoST = (OUTX_NOST[0]+OUTX_NOST[1]+OUTX_NOST[2]+OUTX_NOST[3]+OUTX_NOST[4])/5;
-        outYNoST = (OUTY_NOST[0]+OUTY_NOST[1]+OUTY_NOST[2]+OUTY_NOST[3]+OUTY_NOST[4])/5;
-        outZNoST = (OUTZ_NOST[0]+OUTZ_NOST[1]+OUTZ_NOST[2]+OUTZ_NOST[3]+OUTZ_NOST[4])/5;
-        BusG::write(CTRL5_C, 0x01);
-        miosix::Thread::sleep(200);
-        discard = BusG::read(STATUS_REG);
-        while ((discard & 0x01) == 0 ){
-            discard = BusG::read(STATUS_REG);
-            }
-        BusG::read(OUT_X_L_XL, rxData, 2);
-        BusG::read(OUT_Y_L_XL, rxData, 2);
-        BusG::read(OUT_Z_L_XL, rxData, 2);
-        discard = BusG::read(STATUS_REG);
-        c=0;
-        while ( c<=5 ){
-            while ((discard & 0x01) == 0 ){
-                discard = BusG::read(STATUS_REG);
-            }
-            BusG::read(OUT_X_L_XL, rxData, 2);
-            OUTX_ST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2); 
-            BusG::read(OUT_Y_L_XL, rxData, 2);
-            OUTY_ST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2);
-            BusG::read(OUT_Z_L_XL, rxData, 2);
-            OUTZ_ST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2);
-            c++;
-        }
-        outXST = (OUTX_ST[0]+OUTX_ST[1]+OUTX_ST[2]+OUTX_ST[3]+OUTX_ST[4])/5;
-        outYST = (OUTY_ST[0]+OUTY_ST[1]+OUTY_ST[2]+OUTY_ST[3]+OUTY_ST[4])/5;
-        outZST = (OUTZ_ST[0]+OUTZ_ST[1]+OUTZ_ST[2]+OUTZ_ST[3]+OUTZ_ST[4])/5;
-        float maxSTX = max(OUTX_ST);
-        float maxSTY = max(OUTY_ST);
-        float maxSTZ = max(OUTZ_ST);
-        float minSTX = min(OUTX_ST);
-        float minSTY = min(OUTY_ST);
-        float minSTZ = min(OUTZ_ST);
-        if (((uint8_t)(outXST-outXNoST)<=maxSTX) & ((uint8_t)(outXST-outXNoST)>=minSTX)){
-            if (((uint8_t)(outYST-outYNoST)<=maxSTY) & ((uint8_t)(outYST-outYNoST)>=minSTY)){
-                if (((uint8_t)(outZST-outZNoST)<=maxSTZ) & ((uint8_t)(outZST-outZNoST)>=minSTZ)){
-                    BusG::write(CTRL1_XL, 0x00);
-                    BusG::write(CTRL5_C, 0x00);
-                    return true;
-                }
-            }
-        }
-        BusG::write(CTRL1_XL, 0x00);
-        BusG::write(CTRL5_C, 0x00);
-        return false;
-    }
-    bool gyroSelfTest(){
-        uint8_t discard;
-        uint8_t c=0;
-        uint8_t rxData[2];
-        float OUTX_NOST[]={0, 0, 0, 0, 0};
-        float outXNoST;
-        float OUTY_NOST[]={0, 0, 0, 0, 0};
-        float outYNoST;
-        float OUTZ_NOST[]={0, 0, 0, 0, 0};
-        float outZNoST;
-        float OUTX_ST[]={0, 0, 0, 0, 0};
-        float outXST;
-        float OUTY_ST[]={0, 0, 0, 0, 0};
-        float outYST;
-        float OUTZ_ST[]={0, 0, 0, 0, 0};
-        float outZST;
-        BusG::write(CTRL1_XL, 0x00);
-        BusG::write(CTRL2_G, 0x5C);
-        BusG::write(CTRL3_C, 0x44);
-        BusG::write(CTRL4_C, 0x00);
-        BusG::write(CTRL5_C, 0x00);
-        BusG::write(CTRL6_C, 0x00);
-        BusG::write(CTRL7_G, 0x00);
-        BusG::write(CTRL8_XL, 0x00);
-        BusG::write(CTRL9_XL, 0x00);
-        BusG::write(CTRL10_C, 0x38);
-        miosix::Thread::sleep(800);
-        discard = BusG::read(STATUS_REG);
-         while ((discard & 0x01) == 0 ){
-                discard = BusG::read(STATUS_REG);
-            }
-        BusG::read(OUT_X_L_G, rxData, 2);
-        BusG::read(OUT_Y_L_G, rxData, 2);
-        BusG::read(OUT_Z_L_G, rxData, 2);
-        discard = BusG::read(STATUS_REG);
-        while ( c<=5 ){
-            while ((discard & 0x02) == 0 ){
-                discard = BusG::read(STATUS_REG);
-            }
-            BusG::read(OUT_X_L_G, rxData, 2);
-            OUTX_NOST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2); 
-            BusG::read(OUT_Y_L_G, rxData, 2);
-            OUTY_NOST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2);
-            BusG::read(OUT_Z_L_G, rxData, 2);
-            OUTZ_NOST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2);
-            c++;
-        }
-        outXNoST = (OUTX_NOST[0]+OUTX_NOST[1]+OUTX_NOST[2]+OUTX_NOST[3]+OUTX_NOST[4])/5;
-        outYNoST = (OUTY_NOST[0]+OUTY_NOST[1]+OUTY_NOST[2]+OUTY_NOST[3]+OUTY_NOST[4])/5;
-        outZNoST = (OUTZ_NOST[0]+OUTZ_NOST[1]+OUTZ_NOST[2]+OUTZ_NOST[3]+OUTZ_NOST[4])/5;
-        BusG::write(CTRL5_C, 0x04);
-        miosix::Thread::sleep(60);
-        discard = BusG::read(STATUS_REG);
-        while ((discard & 0x02) == 0 ){
-                discard = BusG::read(STATUS_REG);
-            }
-        BusG::read(OUT_X_L_G, rxData, 2);
-        BusG::read(OUT_Y_L_G, rxData, 2);
-        BusG::read(OUT_Z_L_G, rxData, 2);
-        discard = BusG::read(STATUS_REG);
-        while ( c<=5 ){
-            while ((discard & 0x02) == 0 ){
-                discard = BusG::read(STATUS_REG);
-            }
-            BusG::read(OUT_X_L_G, rxData, 2);
-            OUTX_ST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2); 
-            BusG::read(OUT_Y_L_G, rxData, 2);
-            OUTY_ST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2);
-            BusG::read(OUT_Z_L_G, rxData, 2);
-            OUTZ_ST[c] = toFloat(rxData[0]+(rxData[1]<<8), 2);
-            c++;
-        }
-        outXST = (OUTX_ST[0]+OUTX_ST[1]+OUTX_ST[2]+OUTX_ST[3]+OUTX_ST[4])/5;
-        outYST = (OUTY_ST[0]+OUTY_ST[1]+OUTY_ST[2]+OUTY_ST[3]+OUTY_ST[4])/5;
-        outZST = (OUTZ_ST[0]+OUTZ_ST[1]+OUTZ_ST[2]+OUTZ_ST[3]+OUTZ_ST[4])/5;
-        float maxSTX = max(OUTX_ST);
-        float maxSTY = max(OUTY_ST);
-        float maxSTZ = max(OUTZ_ST);
-        float minSTX = min(OUTX_ST);
-        float minSTY = min(OUTY_ST);
-        float minSTZ = min(OUTZ_ST);
-        if (((uint8_t)(outXST-outXNoST)<=maxSTX) & ((uint8_t)(outXST-outXNoST)>=minSTX)){
-            if (((uint8_t)(outYST-outYNoST)<=maxSTY) & ((uint8_t)(outYST-outYNoST)>=minSTY)){
-                if (((uint8_t)(outZST-outZNoST)<=maxSTZ) & ((uint8_t)(outZST-outZNoST)>=minSTZ)){
-                    BusG::write(CTRL2_G, 0x00);
-                    BusG::write(CTRL5_C, 0x00);
-                    return true;
-                }
-            }
-        }
-        BusG::write(CTRL2_G, 0x00);
-        BusG::write(CTRL5_C, 0x00);
-        return false;
-
-    }
-    float min (float* var){
-        uint8_t min=var[0];
-        uint8_t i=0;
-        for(i=0; i<=5; i++){
-            if (var[i]<= min){
-                min= var[i];
-            }
-        }
-        return min;
-    }
-    float max (float* var){
-        uint8_t max=var[0];
-        uint8_t i=0;
-        for(i=0; i<=5; i++){
-            if (var[i]>= max){
-                max= var[i];
-            }
-        }
-        return max;
-    }
-    
-    float toFloat(uint16_t var, uint16_t FScale){
-        float out;
-        if(var > 0x7FFF){
-            out= var-0xFFFF;
-        }
-        else {
-            out= var;
-        }
-        out = out*FScale/0x8000;
-        return out;
-    }
-    uint8_t datiGyro[6]  = {0, 0, 0, 0, 0, 0};
-    uint8_t datiAccel[6] = {0, 0, 0, 0, 0, 0};
-
-    struct Samples
-    {
-        float gyroXSample;
-        float gyroYSample;
-        float gyroZSample;
-        float accelXSample;
-        float accelYSample;
-        float accelZSample;
-    };
-    Samples Data;
-
-private:
-    uint8_t accelFS;
-    uint16_t gyroFS;
-    constexpr static uint8_t whoami_value = 0x69;
-
-    uint8_t accelFSMAP[4] = {2, 16, 4, 8};
-    uint16_t gyroFSMAP[4]  = {250, 500, 1000, 2000};
-    enum RegMap {
-        WHO_AM_I = 0x0F,  // default value
-        // FIFO configuration registers
-        FIFO_CTRL_REG1 = 0x06,
-        FIFO_CTRL_REG2 = 0x07,
-        FIFO_CTRL_REG3 = 0x08,
-        FIFO_CTRL_REG4 = 0x09,
-        FIFO_CTRL_REG5 = 0x0A,
-
-        // Accelerometer and gyroscope control registers
-        CTRL1_XL = 0x10,
-        CTRL2_G  = 0x11,
-        CTRL3_C  = 0x12,
-        CTRL4_C  = 0x13,
-        CTRL5_C  = 0x14, 
-        CTRL6_C  = 0x15, 
-        CTRL7_G  = 0x16, 
-        CTRL8_XL = 0x17, 
-        CTRL9_XL = 0x18, 
-        CTRL10_C = 0x19,
-
-        // Accelerometer output registers
-
-        OUT_X_L_XL = 0x28,
-        OUT_X_H_XL = 0x29,
-        OUT_Y_L_XL = 0x2A,
-        OUT_Y_H_XL = 0x2B,
-        OUT_Z_L_XL = 0x2C,
-        OUT_Z_H_XL = 0x2D,
-
-        // gyro data registers
-        OUT_X_L_G = 0x22,
-        OUT_X_H_G = 0x23,
-        OUT_Y_L_G = 0x24,
-        OUT_Y_H_G = 0x25,
-        OUT_Z_L_G = 0x26,
-        OUT_Z_H_G = 0x27,
-
-        // FIFO status registers
-        FIFO_STATUS1 = 0x3A,
-        FIFO_STATUS2 = 0x3B,
-        FIFO_STATUS3 = 0x3C,
-        FIFO_STATUS4 = 0x3D,
-
-        // FIFO data output registers
-        FIFO_DATA_OUT_L = 0x3E,
-        FIFO_DATA_OUT_H = 0x3F,
-        // STATUS register
-        STATUS_REG  = 0X1E
-    };
-};
\ No newline at end of file
diff --git a/src/shared/sensors/LSM6DS3H/LSM6DS3H.h b/src/shared/sensors/LSM6DS3H/LSM6DS3H.h
new file mode 100644
index 0000000000000000000000000000000000000000..117e823c2dd526af27a121dbb8b6a6469f652379
--- /dev/null
+++ b/src/shared/sensors/LSM6DS3H/LSM6DS3H.h
@@ -0,0 +1,220 @@
+/*  LSM6DS3H Driver
+ *
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Nuno Barcellos
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef LSM6DS3H_H
+#define LSM6DS3H_H
+
+#include <drivers/BusTemplate.h>
+
+#include "../Sensor.h"
+#include "Common.h"
+#include "miosix.h"
+
+/** ********ATTENTION***********
+ * @warning Once upon a time, this driver was working, but due to hardware
+ * problems we were unable to confirm it and test it properly. If you need this,
+ * you should test it a bit before using it in production code.
+ */
+
+template <typename Bus>
+class LSM6DS3H : public GyroSensor,
+                 public AccelSensor,
+                 public CompassSensor,
+                 public TemperatureSensor
+{
+#pragma pack(1)
+    // __extension__ is needed to prevent compiler warnings for anonymous
+    // structs
+    typedef union {
+        __extension__ struct
+        {
+            int16_t temp;
+            int16_t gyro[3];
+            int16_t accel[3];
+        };
+        int16_t buf[8];
+    } lsmData_t;
+
+#pragma pack()
+
+public:
+    LSM6DS3H(uint8_t accelFullScale, uint16_t gyroFullScale)
+    {
+        accelFS = accelFullScale;
+        gyroFS  = gyroFullScale;
+    }
+
+    bool init() override
+    {
+        // SPI clock frequency up to 10 MHz
+        // The device is compatible with SPI modes 0 and 3
+
+        uint8_t whoami = Bus::read(RegMap::WHO_AM_I);
+        printf("[LSM] expected: %x actual: %x\n", whoami_value, whoami);
+        if (whoami != whoami_value)
+        {
+            last_error = ERR_NOT_ME;
+            return false;
+        }
+
+        // Reset device
+        uint8_t reg = Bus::read(RegMap::CTRL3_C);
+        Bus::write(reg | 0x01);
+
+        miosix::Thread::sleep(100);
+
+        // clang-format off
+        uint8_t init_data[][2] = 
+        {
+            {RegMap::CTRL3_C, 0x44}, // Register address automatically incremented during a multiple 
+                                     // byte access with a serial interface; LSB @ lower address;
+                                     // SPI 4 wire; Output registers not updated until MSB and LSB
+                                     // have been read
+            {RegMap::CTRL1_XL, (uint8_t) (0x70 | (accelFS << 2))}, // Accel ODR to 833 Hz,
+                                                                   // Anti-aliasing filter to 400 hz
+            // {RegMap::CTRL5_C, 0x60}, // Rounding read enabled for both accel and gyro
+            {RegMap::CTRL2_G, (uint8_t) (0x70 | (gyroFS << 2))},   // Gyro ODR to 833 Hz
+        };
+        // clang-format on
+
+        for (size_t i = 0; i < sizeof(init_data) / sizeof(init_data[0]); i++)
+            Bus::write(init_data[i][0], init_data[i][1]);
+
+        return true;
+    }
+
+    bool selfTest() override { return true; }
+
+    bool onSimpleUpdate()
+    {
+        lsmData_t raw_data;
+        uint8_t buf[20];
+
+        // Read temp, gyro, accel
+        Bus::read(RegMap::OUT_TEMP_L, buf, 14);
+        memcpy(&raw_data.buf, buf, 14);
+
+        mLastTemp = normalizeTemp(raw_data.temp);
+
+        mLastGyro.setX(normalizeGyro(raw_data.gyro[0]));
+        mLastGyro.setY(normalizeGyro(raw_data.gyro[1]));
+        mLastGyro.setZ(normalizeGyro(raw_data.gyro[2]));
+
+        mLastAccel.setX(normalizeAccel(raw_data.accel[0]));
+        mLastAccel.setY(normalizeAccel(raw_data.accel[1]));
+        mLastAccel.setZ(normalizeAccel(raw_data.accel[2]));
+
+        return true;
+    }
+
+    // clang-format off
+    enum gyroFullScale
+    {
+        GYRO_FS_250           = 0,
+        GYRO_FS_500           = 1,
+        GYRO_FS_1000          = 2,
+        GYRO_FS_2000          = 3
+    };
+
+    enum accelFullScale
+    {
+        ACC_FS_2G             = 0, // Do not change this sequence
+        ACC_FS_4G             = 2,
+        ACC_FS_8G             = 3,
+        ACC_FS_16G            = 1
+    };
+    // clang-format on
+
+private:
+    constexpr static uint8_t whoami_value = 0x69;
+    constexpr static float accelFSMAP[4]  = {0.061, 0.488, 0.122, 0.244};
+    constexpr static float gyroFSMAP[4]   = {8.75, 17.50, 35.0, 70.0};
+    uint8_t accelFS;
+    uint16_t gyroFS;
+
+    inline float normalizeAccel(int16_t val)
+    {
+        return static_cast<float>(val) / 1000.0f * accelFSMAP[accelFS] *
+               EARTH_GRAVITY;  // [m/ss]
+    }
+
+    inline float normalizeGyro(int16_t val)
+    {
+        return static_cast<float>(val) / 1000.0f * gyroFSMAP[gyroFS] *
+               DEGREES_TO_RADIANS;  // [rad/s]
+    }
+
+    inline float normalizeTemp(int16_t val)
+    {
+        return static_cast<float>(val) / 16.0f + 25.0f;  // [deg C]
+    }
+
+    enum RegMap
+    {
+        WHO_AM_I = 0x0F,  // default value
+
+        // Accelerometer and gyroscope control registers
+        CTRL1_XL = 0x10,
+        CTRL2_G  = 0x11,
+        CTRL3_C  = 0x12,
+        CTRL4_C  = 0x13,
+        CTRL5_C  = 0x14,
+        CTRL6_C  = 0x15,
+        CTRL7_G  = 0x16,
+        CTRL8_XL = 0x17,
+        CTRL9_XL = 0x18,
+        CTRL10_C = 0x19,
+
+        // Temperature output registers
+        OUT_TEMP_L = 0x20,
+        OUT_TEMP_H = 0x21,
+
+        // Gyro data registers
+        OUT_X_L_G = 0x22,
+        OUT_X_H_G = 0x23,
+        OUT_Y_L_G = 0x24,
+        OUT_Y_H_G = 0x25,
+        OUT_Z_L_G = 0x26,
+        OUT_Z_H_G = 0x27,
+
+        // Accelerometer output registers
+        OUT_X_L_XL = 0x28,
+        OUT_X_H_XL = 0x29,
+        OUT_Y_L_XL = 0x2A,
+        OUT_Y_H_XL = 0x2B,
+        OUT_Z_L_XL = 0x2C,
+        OUT_Z_H_XL = 0x2D,
+
+        // STATUS register
+        STATUS_REG = 0X1E
+    };
+};
+
+template <typename Bus>
+constexpr float LSM6DS3H<Bus>::accelFSMAP[];
+
+template <typename Bus>
+constexpr float LSM6DS3H<Bus>::gyroFSMAP[];
+
+#endif /* ifndef LSM6DS3H */
diff --git a/src/shared/sensors/LSM6DS3H/LSM6DS3HData.h b/src/shared/sensors/LSM6DS3H/LSM6DS3HData.h
new file mode 100644
index 0000000000000000000000000000000000000000..b34c3aa815f6c7ac970c84bac755cb1cb94a4439
--- /dev/null
+++ b/src/shared/sensors/LSM6DS3H/LSM6DS3HData.h
@@ -0,0 +1,48 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Nuno Barcellos
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef SRC_SHARED_SENSORS_LSM6DS3H_LSM6DS3HDATA_H
+#define SRC_SHARED_SENSORS_LSM6DS3H_LSM6DS3HDATA_H
+
+#include <ostream>
+#include "math/Vec3.h"
+
+struct LSM6DS3HData
+{
+    long long timestamp;
+    Vec3 accel;
+    Vec3 gyro;
+    float temp;
+
+    static std::string header()
+    {
+        return "timestamp,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << accel.getX() << "," << accel.getY() << ","
+           << accel.getZ() << "," << gyro.getX() << "," << gyro.getY() << ","
+           << gyro.getZ() << "\n";
+    }
+};
+
+#endif /* SRC_SHARED_SENSORS_LSM6DS3H_LSM6DS3HDATA_H */
diff --git a/src/shared/sensors/MPU9250/MPU9250.h b/src/shared/sensors/MPU9250/MPU9250.h
index 5c0e8a7adc986e3db6d61d54537ebe8bf07f0e69..23a2edba773f8d90de53954439bb97d2600bca55 100644
--- a/src/shared/sensors/MPU9250/MPU9250.h
+++ b/src/shared/sensors/MPU9250/MPU9250.h
@@ -113,19 +113,20 @@ public:
     }
 
     bool init() override
-    {
+    {       
+        miosix::Thread::sleep(100);
         uint8_t whoami = Bus::read(REG_WHO_AM_I);
-        // printf("MPU whoami: expected %x actual %x\n", who_am_i_value_mpu, whoami);
-        if (whoami != who_am_i_value_mpu)
+        TRACE("MPU whoami: expected %x actual %x\n", who_am_i_value_mpu, whoami);
+
+        if (whoami != who_am_i_value_mpu && whoami != 0x70)
         {
             last_error = ERR_NOT_ME;
             return false;
         }
-        int i = 0;
-        while(!initMagneto() && i < 10)
+
+        if(!initMagneto())
         {
-            ++i;
-            miosix::Thread::sleep(5);
+            TRACE("MPU9250 Magnetometer init failed\n");
         }
 
         // Initialize MPU9250
diff --git a/src/tests/catch/test-packetqueue.cpp b/src/tests/catch/test-packetqueue.cpp
index 3e8c2f71b60f6d53c95d680de839347c952990c3..58e6bc12145070d0c6cfc177276686592d99de87 100644
--- a/src/tests/catch/test-packetqueue.cpp
+++ b/src/tests/catch/test-packetqueue.cpp
@@ -42,7 +42,7 @@ uint8_t buf[BUF_LEN];
 
 inline bool COMPARE(uint8_t* buf, size_t len, const char* expected)
 {
-    int i = 0;
+    size_t i = 0;
     while (expected[i] != '\0' && i < len)
     {
         CAPTURE(i);
@@ -240,7 +240,7 @@ TEST_CASE("PacketQueue tests")
         COMPARE(pq.buffer.get(1), "abcd");
         COMPARE(pq.buffer.get(2), "abcdefg");
 
-        
+
         INFO("Popping first element");
         p = pq.pop();  // Should still return first packet
         REQUIRE(p.msgCount() == 3);
@@ -263,7 +263,7 @@ TEST_CASE("PacketQueue tests")
         REQUIRE(pq.countNotEmpty() == 3);
         REQUIRE_FALSE(pq.isEmpty());
         REQUIRE(pq.isFull());
-        
+
         COMPARE(pq.buffer.get(0), "abcd");
         COMPARE(pq.buffer.get(1), "abcdefg");
         COMPARE(pq.buffer.get(2), "0123456789");
@@ -298,7 +298,7 @@ TEST_CASE("PacketQueue tests")
         REQUIRE(pq.isEmpty());
         REQUIRE(pq.countNotEmpty() == 0);
         REQUIRE(pq.countReady() == 0);
-        
+
         INFO("Adding empty message")
         REQUIRE(pq.put(message_base, 0) == -1);
 
diff --git a/src/tests/drivers/flashmemory/FlashControllerTests.h b/src/tests/drivers/flashmemory/FlashControllerTests.h
new file mode 100644
index 0000000000000000000000000000000000000000..ded0da2665f4480efee2406f897df87637992016
--- /dev/null
+++ b/src/tests/drivers/flashmemory/FlashControllerTests.h
@@ -0,0 +1,686 @@
+#ifndef FLASHCONTROLLERTESTS_H
+#define FLASHCONTROLLERTESTS_H
+
+#include <Common.h>
+#include <helper/MultiFlashController.h>
+#include <diagnostic/NewLogger.h>
+#include <drivers/timer.h>
+#include <Tests.h>
+#include <vector>
+#include <limits>
+#include <string>
+
+using std::vector;
+using logging::logger;
+using namespace flashmemory;
+
+namespace testing
+{
+namespace flashmemorytests
+{
+
+template<typename MemorySPI>
+class FlashTest : public Test
+{
+ public:
+
+  FlashTest(MultiFlashController* multi)
+      : Test(), multi(multi), controller(multi->flash0_), driver(
+          controller->flash_)
+  {
+
+  }
+ protected:
+  typedef FlashController<MemorySPI> FlashControllerType;
+
+  void controller_setBlockIndex(uint32_t block_index)
+  {
+    controller->block_index_ = block_index;
+  }
+
+  uint32_t controller_getBlockIndex()
+  {
+    return controller->block_index_;
+  }
+
+  bool controller_format()
+  {
+#ifdef FLASH_TEST
+    return FlashControllerType::format();
+#else
+    return FlashControllerType::fastFormat();
+#endif
+  }
+
+  void controller_init()
+  {
+    controller->init();
+  }
+
+  bool controller_readSectorHeader(uint32_t sector, SectorHeader* header)
+  {
+    return FlashControllerType::readSectorHeader(sector, header);
+  }
+
+  bool controller_readBlock(uint32_t block_index, uint8_t* buffer)
+  {
+    return FlashControllerType::readBlock(block_index, buffer);
+  }
+
+  bool controller_readDataBlock(uint32_t block_index, FlashDataBlock* block)
+  {
+    return FlashControllerType::readDataBlock(block_index, block);
+  }
+
+  bool controller_writeBlock(FlashDataBlock* block)
+  {
+    return controller->writeDataBlock(block);
+  }
+
+  bool controller_writeSectorHeader(uint32_t sector, SectorHeader& sheader)
+  {
+    return FlashControllerType::writeSectorHeader(sector, sheader);
+  }
+
+  MultiFlashController* multi;
+  FlashController<MemorySPI>* controller;
+  FlashDriver<MemorySPI>* driver;
+};
+
+/*
+ * Copy this to create a new FlashTest
+ *
+
+ template<typename MemorySPI>
+ class EmptyFlashTest : public FlashTest<MemorySPI>
+ {
+ typedef FlashTest<MemorySPI> Base;
+
+ public:
+
+ EmptyFlashTest(MultiFlashController* multi)
+ : Base::FlashTest(multi)
+ {
+
+ }
+
+ bool setup()
+ {
+ return true;
+ }
+
+ bool run()
+ {
+ return true;
+ }
+
+ string name()
+ {
+ return "EmptyFlashTest";
+ }
+ };
+
+ */
+
+class CompareFlashHeaderTest : public Test
+{
+ public:
+
+  CompareFlashHeaderTest()
+      : Test()
+  {
+
+  }
+
+  bool run()
+  {
+    bool success = true;
+    FlashHeader header1, header2;
+
+    bool test = header1 == header2;
+    success = success && test;
+    logger.info(logtag(), "Compare equal headers: ", (test ? "OK" : "Error"));
+
+    header1.read_only = 54;
+
+    test = header1 == header2;
+    success = success && (test == false);
+    logger.info(logtag(), "Compare different headers: ",
+                (test == false ? "OK" : "Error"));
+
+    return success;
+  }
+
+  string name()
+  {
+    return "CompareFlashHeaderTest";
+  }
+
+ private:
+  static string logtag()
+  {
+    return "CompareFHTest";
+  }
+};
+
+template<typename MemorySPI>
+class MeasureRebootTimeTest : public FlashTest<MemorySPI>
+{
+  typedef FlashTest<MemorySPI> Base;
+  typedef Timer32<2> Timer2;
+ public:
+  MeasureRebootTimeTest(MultiFlashController* multi)
+      : Base::FlashTest(multi)
+  {
+
+  }
+
+  bool run()
+  {
+#ifdef FLASH_TEST
+    logger.warning(logtag(), "You are using the mockup memory!");
+#endif
+    logger.info(logtag(), "Running 100 read repetitions!");
+    uint8_t result;
+    SectorHeader header;
+    float avg, min = std::numeric_limits<float>::max(), max = 0;
+    Timer2::start();
+    for (int j = 0; j < 100; j++)
+    {
+      uint32_t t0 = Timer2::tick();
+      for (uint32_t i = 0; i < SECTORS_NUM; i++)
+      {
+        Base::driver->read(&result, i * SECTOR_SIZE, (uint8_t*) (&header),
+                           sizeof(SectorHeader));
+        if (result != RESULT_OK)
+        {
+          break;
+        }
+      }
+      uint32_t t1 = Timer2::tick();
+      float t = Timer2::milliSeconds(t1 - t0);
+      avg += t;
+      if (t < min)
+      {
+        min = t;
+      } else if (t > max)
+      {
+        max = t;
+      }
+    }
+    Timer2::stop();
+    avg = avg / 100;
+    if (result == RESULT_OK)
+    {
+      logger.info(logtag(), "Avg time: ", avg, " ms");
+      logger.info(logtag(), "Min time: ", min, " ms, Max time: ", max, " ms");
+      return true;
+    } else
+    {
+      return false;
+    }
+  }
+
+  string name()
+  {
+    return "MeasureWorstRebootTimeTest";
+  }
+
+ private:
+  static string logtag()
+  {
+    return "RebootTimeTest";
+  }
+};
+
+template<typename MemorySPI>
+class FlashHeaderTest : public FlashTest<MemorySPI>
+{
+  typedef FlashTest<MemorySPI> Base;
+  //using Base::driver;
+  //using Base::controller;
+
+ public:
+
+  FlashHeaderTest(MultiFlashController* multi)
+      : Base::FlashTest(multi)
+  {
+
+  }
+
+  bool setup()
+  {
+    return cleanFirstSubsector();
+  }
+
+  bool run()
+  {
+    FlashHeader orig_header, read_header;
+    generateRandomHeader(&orig_header);
+    orig_header.first_boot = FIRST_BOOT_KEY;
+    logger.info(logtag(), "--SUBTEST 1: Write & read over clean memory");
+
+    if (!writeAndReadHeader(orig_header, read_header, false))
+    {
+      return false;
+    }
+    if (orig_header != read_header)
+    {
+      logger.error(logtag(), "Original & written headers are not the same!");
+      return false;
+    }
+    logger.info(logtag(), "Success.");
+
+    orig_header.first_boot = 0;
+    logger.info(logtag(), "--SUBTEST 2: Overwrite first_boot");
+
+    if (!writeAndReadHeader(orig_header, read_header, false))
+    {
+      return false;
+    }
+    if (orig_header != read_header)
+    {
+      logger.error(logtag(), "Original & written headers are not the same!");
+      return false;
+    }
+    logger.info(logtag(), "Success.");
+
+    logger.info(logtag(), "--SUBTEST 3: Write & read over dirty memory");
+    FlashHeader dirtywrite_header;
+    generateRandomHeader(&dirtywrite_header);
+
+    if (Base::FlashControllerType::writeFlashHeader(dirtywrite_header, false))
+    {
+      logger.error(
+          logtag(),
+          "Function writeFlashHeader should have returned RESULT_CHECK_FAIL!");
+      return false;
+    }
+    logger.info(logtag(), "Success.");
+
+    logger.info(logtag(),
+                "--SUBTEST 4: Write with erase & read over dirty memory");
+    generateRandomHeader(&dirtywrite_header);
+
+    if (!writeAndReadHeader(dirtywrite_header, read_header, true))
+    {
+      return false;
+    }
+    if (dirtywrite_header != read_header)
+    {
+      logger.error(logtag(), "Original & written headers are not the same!");
+      return false;
+    }
+    logger.info(logtag(), "Success.");
+    return true;
+  }
+
+  string name()
+  {
+    return "WriteFlashHeaderTest";
+  }
+
+ private:
+  static string logtag()
+  {
+    return "WrtFHeaderTest";
+  }
+
+  bool cleanFirstSubsector()
+  {
+    uint8_t result;
+    Base::driver->enableErase();
+    Base::driver->eraseSubsector(&result, 0);
+
+    return result == RESULT_OK;
+  }
+
+  void generateRandomHeader(FlashHeader* header)
+  {
+    header->start_index = (uint32_t) rand();
+    header->read_only = (uint32_t) rand();
+    header->first_boot = (uint32_t) rand();
+  }
+
+  bool writeAndReadHeader(FlashHeader& write_header, FlashHeader& read_header,
+                          bool erase)
+  {
+    if (!Base::FlashControllerType::writeFlashHeader(write_header, erase))
+    {
+      logger.error(logtag(), "Error writing header");
+      return false;
+    }
+
+    if (!Base::FlashControllerType::readFlashHeader(&read_header))
+    {
+      logger.error(logtag(), "Error reading header");
+      return false;
+    }
+
+    return true;
+  }
+};
+
+template<typename MemorySPI>
+class FlashBlockTest : public FlashTest<MemorySPI>
+{
+  typedef FlashTest<MemorySPI> Base;
+
+ public:
+
+  FlashBlockTest(MultiFlashController* multi)
+      : Base::FlashTest(multi)
+  {
+
+  }
+
+  bool setup()
+  {
+    if (Base::controller_format())
+    {
+      Base::controller_init();
+      return true;
+    }
+    return false;
+  }
+
+  bool run()
+  {
+    logger.info(logtag(), "--SUBTEST 1: Just write and read");
+    if (!subtest1())
+      return false;
+    logger.info(logtag(), "Success.");
+
+    logger.info(logtag(), "--SUBTEST 2: Write near the end of a sector");
+    if (!subtest2())
+      return false;
+    logger.info(logtag(), "Success.");
+
+    logger.info(logtag(), "--SUBTEST 3: Write at the end of the memory");
+    if (!subtest3())
+      return false;
+    logger.info(logtag(), "Success.");
+
+    return true;
+  }
+
+  string name()
+  {
+    return "FlashBlockTest";
+  }
+
+ private:
+  static string logtag()
+  {
+    return "FBlockTest";
+  }
+
+  bool subtest1()
+  {
+    uint32_t block_index = SUBSECTOR_SIZE / BLOCK_SIZE;
+
+    Base::controller_setBlockIndex(block_index);
+    FlashDataBlock write_block, read_block;
+    generateDataBlock(&write_block);
+
+    if (!writeBlock(&write_block))
+    {
+      logger.error(logtag(), "Cannot write block");
+      return false;
+    }
+
+    if (!readBlock(block_index, &read_block))
+    {
+      logger.error(logtag(), "Cannot read block");
+      return false;
+    }
+
+    if (read_block != write_block)
+    {
+      logger.error(logtag(), "Written & read block do not match!");
+      return false;
+    }
+    return true;
+  }
+
+  bool subtest2()
+  {
+    uint32_t block_index = SECTOR_SIZE / BLOCK_SIZE - 1;
+
+    Base::controller_setBlockIndex(block_index);
+
+    FlashDataBlock write_blocks[2];
+    FlashDataBlock read_blocks[2];
+    for (int i = 0; i < 2; i++)
+    {
+      generateDataBlock(&write_blocks[i]);
+      if (!writeBlock(&write_blocks[i]))
+      {
+        logger.error(logtag(), "Cannot write block ", i + 1);
+        return false;
+      }
+    }
+
+    SectorHeader sheader;
+    if (!Base::controller_readSectorHeader(1, &sheader))
+    {
+      logger.error(logtag(), "Cannot read sector header");
+      return false;
+    }
+
+    if (sheader.content != SECTOR_WRITTEN)
+    {
+      logger.error(logtag(), "Sector header not correctly written");
+      logger.error(logtag(), "Sector header content: 0x%02X; should be: 0x%02X",
+                   sheader.content, SECTOR_WRITTEN);
+      return false;
+    }
+
+    if (!readBlock(block_index, &read_blocks[0]))
+    {
+      logger.error(logtag(), "Cannot read block 1");
+      return false;
+    }
+
+    if (!readBlock(block_index + 2, &read_blocks[1]))
+    {
+      logger.error(logtag(), "Cannot read block 2");
+      return false;
+    }
+
+    if (!compareBlocks(write_blocks, read_blocks, 2))
+    {
+      logger.error(logtag(), "Written & read blocks do not match.");
+      return false;
+    }
+    return true;
+  }
+
+  bool subtest3()
+  {
+    //Last block of the memory
+    uint32_t block_index = MEMORY_SIZE / BLOCK_SIZE - 1;
+    Base::controller_setBlockIndex(block_index);
+
+    //Keep the filesystem consistent.
+    SectorHeader sheader;
+    sheader.content = SECTOR_WRITTEN;
+    Base::controller_writeSectorHeader(MEMORY_SIZE / SECTOR_SIZE - 1, sheader);
+
+    FlashDataBlock write_blocks[2];
+    FlashDataBlock read_blocks[2];
+
+    for (int i = 0; i < 2; i++)
+    {
+      generateDataBlock(&write_blocks[i]);
+    }
+
+    if (!writeBlock(&write_blocks[0]))
+    {
+      logger.error(logtag(),
+                   "Write block failed on the last available address!");
+      return false;
+    }
+
+    if (writeBlock(&write_blocks[1]))
+    {
+      logger.error(
+          logtag(),
+          "Write block didn't fail while writing outside of the memory!");
+      return false;
+    }
+    if (!readBlock(block_index, &read_blocks[0]))
+    {
+      logger.error(logtag(), "Cannot read block 1");
+      return false;
+    }
+
+    if (readBlock(block_index + 2, &read_blocks[1]))
+    {
+      logger.error(
+          logtag(),
+          "readBlock didn't fail while reading outside of the memory!");
+      return false;
+    }
+
+    if (read_blocks[0] != write_blocks[0])
+    {
+      logger.error(logtag(), "Written & read blocks do not match.");
+      return false;
+    }
+
+    return true;
+  }
+
+  bool writeBlock(FlashDataBlock* block)
+  {
+    return Base::controller_writeBlock(block);
+  }
+
+  bool readBlock(uint32_t block_index, FlashDataBlock* block)
+  {
+    return Base::controller_readBlock(block_index, (uint8_t*) (block));
+  }
+
+  bool compareBlocks(FlashDataBlock* blocks1, FlashDataBlock* blocks2,
+                     size_t num_blocks)
+  {
+    for (size_t i = 0; i < num_blocks; i++)
+    {
+      if (blocks1[i] != blocks2[i])
+      {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  void generateDataBlock(FlashDataBlock* generated)
+  {
+    generated->id = (uint32_t) rand();
+    generated->crc = (uint16_t) rand();
+    generated->timestamp = (uint32_t) rand();
+
+    for (int i = 0; i < FLASH_BUFFER_SIZE; i++)
+    {
+      generated->buffer[i] = (uint8_t) (rand() % 256);
+    }
+  }
+};
+
+template<typename MemorySPI>
+class MultiFlashCTRLTest : public FlashTest<MemorySPI>
+{
+  typedef FlashTest<MemorySPI> Base;
+
+ public:
+
+  MultiFlashCTRLTest(MultiFlashController* multi)
+      : Base::FlashTest(multi)
+  {
+
+  }
+
+  bool setup()
+  {
+    if (!Base::controller_format())
+    {
+      return false;
+    }
+    Base::controller_init();
+    if (Base::controller->getStatus()
+        != Base::FlashControllerType::Status::WRITE_READY)
+    {
+      logger.error(logtag(), "Error initializing flash controller.");
+      return false;
+    }
+    return true;
+  }
+
+  bool run()
+  {
+    logger.info(logtag(), "--SUBTEST 1: Write @ beginning");
+    if (!subtest1())
+      return false;
+    logger.info(logtag(), "Success.");
+
+    return true;
+  }
+
+  string name()
+  {
+    return "EmptyFlashTest";
+  }
+
+ private:
+  bool subtest1()
+  {
+    uint32_t index = Base::controller_getBlockIndex();
+
+    uint8_t rand_buff1[FLASH_BUFFER_SIZE];
+    uint8_t rand_buff2[FLASH_BUFFER_SIZE];
+    Base::multi->addData(rand_buff1, FLASH_BUFFER_SIZE);
+    Base::multi->addData(rand_buff2, FLASH_BUFFER_SIZE);
+
+    Thread::sleep(500); //Wait for data to be written
+
+    logger.info(logtag(), "Reading blocks.");
+    FlashDataBlock read_block;
+
+    if(!Base::controller_readDataBlock(index, &read_block))
+    {
+      logger.error(logtag(), "Error reading first block.");
+      return false;
+    }
+
+    if(memcmp(rand_buff1, &(read_block.buffer), FLASH_BUFFER_SIZE) != 0)
+    {
+      logger.error(logtag(), "Read block 1 is not the same as written one.");
+      return false;
+    }
+
+    if(!Base::controller_readDataBlock(index+1, &read_block))
+    {
+      logger.error(logtag(), "Error reading second block.");
+      return false;
+    }
+
+    if(memcmp(rand_buff2, &(read_block.buffer), FLASH_BUFFER_SIZE) != 0)
+    {
+      logger.error(logtag(), "Read block 2 is not the same as written one.");
+      return false;
+    }
+
+    return true;
+  }
+
+  static string logtag()
+  {
+    return "MultiFlashCTRLTest";
+  }
+};
+
+}   //namespace flashmemorytests
+}   //namespace testing
+
+#endif /*FLASHCONTROLLERTESTS_H*/
diff --git a/src/tests/drivers/flashmemory/mockup/FlashDriver.h b/src/tests/drivers/flashmemory/mockup/FlashDriver.h
new file mode 100644
index 0000000000000000000000000000000000000000..18e94c176aa194a65053d6ae82ded4b63a68c9ad
--- /dev/null
+++ b/src/tests/drivers/flashmemory/mockup/FlashDriver.h
@@ -0,0 +1,598 @@
+#ifndef FLASHDRIVER_H
+#define FLASHDRIVER_H
+
+#include <miosix.h>
+#include <Singleton.h>
+#include <diagnostic/NewLogger.h>
+
+using miosix::Thread;
+using miosix::Mode;
+using logging::logger;
+
+//Forward declaration
+namespace testing
+{
+namespace flashmemorytests
+{
+template<typename>
+class FlashTest;
+}
+}
+
+namespace flashmemory
+{
+
+static const uint32_t PAGES_PER_SUBSECTOR = 4U;
+static const uint32_t PAGES_PER_SECTOR = 16U;
+static const uint32_t SUBSECTORS_PER_SECTOR = 4U;
+
+static const uint32_t SUBSECTORS_NUM = 48U;
+static const uint32_t SECTORS_NUM = 12U;
+static const uint32_t PAGE_SIZE = 256U;
+
+static const uint32_t SUBSECTOR_SIZE = PAGE_SIZE * PAGES_PER_SUBSECTOR;
+static const uint32_t SECTOR_SIZE = PAGE_SIZE * PAGES_PER_SECTOR;
+
+static const uint32_t BANK_SIZE = SECTOR_SIZE * SECTORS_NUM / 2;
+static const uint32_t MEMORY_SIZE = SECTOR_SIZE * SECTORS_NUM;
+
+/**
+ *  @brief Definitions for results of various flash operations.
+ *
+ *  Definitions for result flags of various flash operations.
+ */
+
+enum OpResultFlags
+{
+  RESULT_OK         = 0x00,
+
+
+  RESULT_F_OUT_OF_MEMORY  = 0x01,
+  RESULT_F_CHECK_FAIL     = 0x04,
+
+
+  //Bits reserved for future use
+  RESULT_F_UNUSED_1     = 0x08,
+  RESULT_F_UNUSED_2     = 0x40,
+  RESULT_F_UNUSED_3     = 0x80,
+
+
+  /**
+   * These bits are in the same positions of the corresponding bits in the
+   * FLAG STATUS REGISTER
+   */
+  RESULT_F_PROTECTION_ERROR = 0x02,
+  RESULT_F_PROGRAM_ERROR  = 0x10,
+  RESULT_F_ERASE_ERROR    = 0x20
+};
+
+template<typename Bus>
+class FlashDriver  : Singleton<FlashDriver<Bus>>
+{
+  typedef Singleton<FlashDriver<Bus>> SingletonType;
+
+  friend class Singleton<FlashDriver<Bus>>;
+
+  template<typename >
+  friend class FlashDriverTest;
+
+  template<typename >
+  friend class testing::flashmemorytests::FlashTest;
+
+ public:
+
+  ~FlashDriver()
+  {
+    delete fake_memory_;
+  }
+
+  /**
+   * @brief Read n bytes into a buffer, starting from the specified address
+   *
+   * @param result
+   * @param address Starting address
+   * @param buf Buffer to read data into
+   * @param size Number of bytes to read
+   */
+  void read(uint8_t *result, uint32_t address, uint8_t* buf, uint32_t size)
+  {
+    if (address + size > MEMORY_SIZE)
+    {
+      logger.error(logtag(), "Read outside of memory bounds addr + size: ",
+                   address + size, " MEMSIZE: ", MEMORY_SIZE);
+      *result = RESULT_F_OUT_OF_MEMORY;
+      return;
+    }
+
+    uint32_t rsize;
+    if (address < BANK_SIZE)
+      rsize = std::min(size, BANK_SIZE - address);
+    else
+      rsize = size;
+
+    //uint8_t addr_buf[4];
+    //addr_to_buf(addr_buf, address);
+
+    fakeRead(address, buf, rsize);
+    //Bus::read(READ, addr_buf, buf, 4, rsize);
+
+    size -= rsize;
+
+    //This means that we reached the end of the first die but we still need
+    //to read some data.
+    if (size > 0)
+    {
+      address += rsize;
+      buf += rsize;
+
+      //addr_to_buf(addr_buf, address);
+
+      //Read the remaining bytes
+      fakeRead(address, buf, size);
+      //Bus::read(READ, addr_buf, buf, 4, size);
+    }
+
+    *result = RESULT_OK;
+  }
+
+  /**
+   * @brief Writes data on the flash starting from the specified address.
+   *
+   * @param address
+   * @param data
+   * @param size
+   */
+  void write(uint8_t *result, uint32_t address, uint8_t* data, uint32_t size)
+  {
+    //Do not write outside of the memory
+    if (address + size > MEMORY_SIZE)
+    {
+      *result = RESULT_F_OUT_OF_MEMORY;
+      return;
+    }
+
+    *result = RESULT_OK;
+
+    //uint8_t addr_buf[4], status;
+
+    uint32_t next_page, wsize;
+
+    do
+    {
+      next_page = (address / 256) * 256 + 256;
+
+      wsize = std::min(next_page - address, size);
+
+      //addr_to_buf(addr_buf, address);
+      //_write_enable();
+
+      fakeWrite(address, data, wsize);
+      /*Bus::write(PROGRAM_PAGE, addr_buf, data, 4, wsize);
+
+       do {
+       status = _read_flag_status_reg();
+       }while((status & FSR_PRG_ERS_CTRL) == 0);*/
+
+      data += wsize;
+      size -= wsize;
+      address = next_page;
+
+    } while (size > 0 && *result == RESULT_OK);
+
+    /*//Clear flag status register if an error occurred
+     if(*result != RESULT_OK)
+     {
+     _clear_flag_status_reg();
+
+     //Manually reset write_enable latch
+     if((*result & RESULT_PROTECTION_ERROR) > 0)
+     {
+     _write_disable();
+     }
+     }*/
+  }
+
+  /**
+   * @brief Writes data on the flash starting from the specified address, then
+   * checks if everything was written correctly.
+   *
+   * @param address
+   * @param data
+   * @param size
+   */
+  void writeAndCheck(uint8_t *result, uint32_t address, uint8_t* data,
+                     uint32_t size)
+  {  //TODO: size_t
+    write(result, address, data, size);
+    if (*result == RESULT_OK)
+    {
+      uint8_t *check = new uint8_t[size];
+      read(result, address, check, size);
+      for (uint32_t i = 0; i < size; i++)
+      {
+        if (*check++ != *data++)
+        {
+          //Something was not written/read correctly.
+          *result = RESULT_F_CHECK_FAIL;
+          break;
+        }
+      }
+    }
+  }
+
+  /**
+   * @brief Writes data on the flash starting from the specified address, only
+   * if the provided data fits in a single page.
+   *
+   * Writes data on the flash starting from the specified address, only
+   * if the provided data fits in a single page. If too much data is provided
+   * nothing will be written and result will be set to FLASH_OUT_OF_RANGE.
+   *
+   * @param result
+   * @param address
+   * @param data
+   * @param size
+   */
+  void programPage(uint8_t *result, uint32_t address, uint8_t* data,
+                   uint32_t size)
+  {
+    /*//Do now wrap around a page and do not try to write outside of the memory
+     uint32_t next_page = (address / 256)*256 + 256;
+     if(address + size > next_page || address > MEMORY_SIZE)
+     {
+     *result = RESULT_OUT_OF_RNG;
+     return;
+     }
+
+     uint8_t addr_buf[4], status;
+     addr_to_buf(addr_buf, address);
+
+     _write_enable();
+     Bus::write(PROGRAM_PAGE, addr_buf, data, 4, size);
+
+     do {
+     status = _read_flag_status_reg();
+     }while((status & FSR_PRG_ERS_CTRL) == 0);
+
+     *result = status & (FSR_PROGRAM | FSR_PROTECTION);
+
+     //Clear flag status register if an error has been encountered
+     if(*result != RESULT_OK)
+     {
+     _clear_flag_status_reg();
+
+     //Manually reset write_enable latch
+     if((*result & RESULT_PROTECTION_ERROR) > 0)
+     {
+     _write_disable();
+     }
+     }*/
+  }
+
+  /**
+   * @brief Enables or disables read only mode
+   */
+  void setReadOnly(bool readonly = true)
+  {
+    read_only_ = readonly;
+  }
+
+  bool isReadOnly() const
+  {
+    return read_only_;
+  }
+
+  /**
+   * @brief Enable the next erase operation. Call this before every erase op.
+   */
+  void enableErase()
+  {
+    m_erase_enable_ = true;
+  }
+
+  /**
+   * @brief Erases the subsector containing the specified address. For the operation
+   * to be successful, enable_erase_op must be called before this.
+   *
+   * @warning Estimated execution time is slightly less than 1 second.
+   *
+   * @param result
+   * @param address
+   */
+  void eraseSubsector(uint8_t* result, uint32_t address)
+  {
+    erase(result, SUBSECTOR_ERASE, address);
+  }
+
+  /**
+   * @brief Erases the sector containing the specified address. For the operation
+   * to be successful, enable_erase_op must be called before this.
+   * @warning Estimated execution time is about 2 seconds.
+   *
+   * @param result
+   * @param address
+   */
+  void eraseSector(uint8_t* result, uint32_t address)
+  {
+    erase(result, SECTOR_ERASE, address);
+  }
+
+  /**
+   * @brief Erases the entire die containing the specified address. For the operation
+   * to be successful, enable_erase_op must be called before this.
+   * @warning Estimated execution time is more than 4 minutes.
+   *
+   * @param result
+   * @param die: 0 to erase the first die, 1 to erase the second.
+   */
+  void eraseDie(uint8_t* result, uint8_t die)
+  {
+    if (die == 0)
+    {
+      erase(result, DIE_ERASE, 0);
+    } else if (die == 1)
+    {
+      erase(result, DIE_ERASE, BANK_SIZE);
+    }
+  }
+
+  /**
+   * Reads id information for this flash memory.
+   * @param buf Buffer with at least 20 bytes
+   */
+  void readId(uint8_t* buf)
+  {
+    //Bus::read(READ_ID, buf, 20);
+  }
+
+  /**
+   * @brief Software reset for the memory. All volatile bits are set to their
+   * default value.
+   */
+  void softReset()
+  {
+    /*Bus::write(RESET_ENABLE);
+     usleep(5);
+     Bus::write(RESET_MEMORY);
+     _wait_until_ready();*/
+  }
+
+ private:
+
+  FlashDriver()
+  {
+    printf("Mock memory size: %d\n", (int) MEMORY_SIZE);
+    memset((void*) (fake_memory_), 0xFF, MEMORY_SIZE);
+  }
+
+  void erase(uint8_t *result, uint8_t erase_cmd, uint32_t address)
+  {
+    if (address > MEMORY_SIZE)
+    {
+      *result = RESULT_F_OUT_OF_MEMORY;
+      return;
+    }
+
+    if (!m_erase_enable_)
+    {
+      *result = RESULT_F_ERASE_ERROR;
+      return;
+    }
+
+    //uint8_t addr_buf[4], status;
+    //addr_to_buf(addr_buf, address);
+    *result = RESULT_OK;
+    switch (erase_cmd)
+    {
+      case SUBSECTOR_ERASE:
+        address = address / SUBSECTOR_SIZE * SUBSECTOR_SIZE;
+        fakeErase(address, SUBSECTOR_SIZE);
+        break;
+      case SECTOR_ERASE:
+        address = address / SECTOR_SIZE * SECTOR_SIZE;
+        fakeErase(address, SECTOR_SIZE);
+        break;
+      case DIE_ERASE:
+        address = address / BANK_SIZE * BANK_SIZE;
+        fakeErase(address, BANK_SIZE);
+        break;
+      default:
+        *result = RESULT_F_ERASE_ERROR;
+    }
+
+    m_erase_enable_ = false;
+  }
+
+  /**
+   * @brief Checks if a program operation is still in progress.
+   * @param result
+   */
+  bool isBusy()
+  {
+    return false;		//_read_flag_status_reg() & FSR_PRG_ERS_CTRL;
+  }
+
+  /**
+   * @brief Waits until the memory is ready to perform a new write/erase op.
+   * @param result
+   */
+  void waitUntilReady()
+  {
+    /*while((_read_flag_status_reg() & FSR_PRG_ERS_CTRL) == 0)
+     {
+
+     }*/
+  }
+
+  uint8_t readStatusReg()
+  {
+    return 0;
+    /*uint8_t ret = Bus::read(READ_STATUS_REG);
+     return ret;*/
+  }
+
+  void writeStatusReg(uint8_t val)
+  {
+    /*_write_enable();
+     Bus::write(WRITE_STATUS_REG, val);
+     _write_disable();
+     _wait_until_ready();*/
+  }
+
+  uint8_t readFlagStatusReg()
+  {
+    /*uint8_t ret = Bus::read(READ_FLAG_STATUS_REG);
+     return ret;*/
+    return 0;
+  }
+
+  void clearFlagStatusReg()
+  {
+    //Bus::write(CLEAR_FLAG_STATUS_REG);
+  }
+
+  uint16_t readConfigReg()
+  {
+    /*uint8_t buf[2];
+     Bus::read(READ_NV_CONFIG_REG, buf, 2);
+
+     uint16_t res = buf[0] | (buf[1] << 8);*/
+
+    return 0;
+  }
+
+  void writeConfigReg(uint16_t val)
+  {
+    /*
+     * Mask some of the bits of the config register because
+     * we don't want to change them by accident.
+     */
+    /*val = val | 0x0FEC;
+
+     uint8_t buf[2];
+     buf[0] = val & 0xFF;
+     buf[1] = (val >> 8) & 0xFF;
+
+     _write_enable();
+     Bus::write(WRITE_NV_CONFIG_REG, buf, 2);
+     _write_disable();
+     _wait_until_ready();*/
+  }
+
+  void writeEnable()
+  {
+    /*if(!m_read_only) {
+     Bus::write(WRITE_ENABLE);
+     }*/
+  }
+
+  void writeDisable()
+  {
+    //Bus::write(WRITE_DISABLE);
+  }
+
+  static void addrToBuf(uint8_t* buf, uint32_t addr)
+  {
+    for (int i = 0; i < 4; i++)
+    {
+      buf[3 - i] = (addr >> 8 * i) & 0xFF;
+    }
+  }
+
+  void fakeRead(uint32_t address, uint8_t* data, uint32_t size)
+  {
+    //printf("Reading %d bytes from %d \n", (int)size, (int)address);
+    if (address + size > MEMORY_SIZE)
+    {
+      return;
+    }
+
+    memcpy((void*) (data), (void*) (fake_memory_ + address), size);
+  }
+
+  void fakeWrite(uint32_t address, uint8_t* data, uint32_t size)
+  {
+    if (address + size > MEMORY_SIZE)
+    {
+      return;
+    }
+
+    for (uint32_t i = 0; i < size; i++)
+    {
+      fake_memory_[address + i] &= data[i];
+    }
+
+    //memcpy((void*)(fake_memory_ + address), (void*)(data), size);
+  }
+
+  void fakeErase(uint32_t address, uint32_t size)
+  {
+    if (address + size > MEMORY_SIZE)
+    {
+      return;
+    }
+
+    memset((void*) (fake_memory_ + address), 0xFF, size);
+  }
+
+  static std::string logtag()
+  {
+    return "FlashDriver";
+  }
+
+  enum Commands
+  {
+    READ_ID = 0x9F,
+
+    WRITE_ENABLE = 0x06,
+    WRITE_DISABLE = 0x04,
+
+    READ_STATUS_REG = 0x05,
+    WRITE_STATUS_REG = 0x01,
+
+    READ_FLAG_STATUS_REG = 0x70,
+    CLEAR_FLAG_STATUS_REG = 0x50,
+
+    //non-volatile configuration register
+    READ_NV_CONFIG_REG = 0xB5,
+    WRITE_NV_CONFIG_REG = 0xB1,
+
+    //volatile configuration register
+    WRITE_VOL_CONFIG_REG = 0x85,
+    READ_VOL_CONFIG_REG = 0x81,
+
+    READ_EXT_ADDRESS_REG = 0xC8,
+    WRITE_EXT_ADDRESS_REG = 0xC5,
+
+    RESET_ENABLE = 0x66,
+    RESET_MEMORY = 0x99,
+
+    READ = 0x03,
+    PROGRAM_PAGE = 0x02,
+
+    SUBSECTOR_ERASE = 0x20,
+    SECTOR_ERASE = 0xD8,
+    DIE_ERASE = 0xC4
+  };
+
+  uint8_t* fake_memory_ = new uint8_t[MEMORY_SIZE];
+  bool read_only_ = false;
+  bool m_erase_enable_ = false;
+
+  /**
+   * Flag status register bit definitions
+   */
+  static const uint8_t FSR_ADDRESSING_MODE = 0x01;
+  static const uint8_t FSR_PROTECTION = 0x02;
+  static const uint8_t FSR_PROGRAM_SUSPEND = 0x04;
+  static const uint8_t FSR_VPP = 0x08;
+  static const uint8_t FSR_PROGRAM = 0x10;
+  static const uint8_t FSR_ERASE = 0x20;
+  static const uint8_t FSR_ERASE_SUSPEND = 0x40;
+  static const uint8_t FSR_PRG_ERS_CTRL = 0x80;
+};
+
+}   //namespace flashmemory
+
+
+#endif /* FLASHDRIVER_H */
diff --git a/src/tests/drivers/test-IMU.cpp b/src/tests/drivers/test-IMU.cpp
index 06e5aadd8972947e504ddfc34942c0c385c692ce..ee8fe8702c820c816c48b56c0b779329c8e2fad4 100644
--- a/src/tests/drivers/test-IMU.cpp
+++ b/src/tests/drivers/test-IMU.cpp
@@ -1,4 +1,4 @@
-#include <sensors/LSM6DS3H.h>
+#include <sensors/LSM6DS3H/LSM6DS3H.h>
 
 // SPI1
 typedef miosix::Gpio<GPIOA_BASE, 5> GpioSck;
diff --git a/src/tests/drivers/test-lsm.cpp b/src/tests/drivers/test-lsm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..641d490a1510c60bd3ae1fa42ac2a88aeaf4476a
--- /dev/null
+++ b/src/tests/drivers/test-lsm.cpp
@@ -0,0 +1,82 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Nuno Barcellos
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Common.h>
+#include <drivers/BusTemplate.h>
+#include <interfaces-impl/hwmapping.h>
+#include <sensors/LSM6DS3H/LSM6DS3H.h>
+
+#include <drivers/spi/SensorSpi.h>
+#include <sensors/SensorSampling.h>
+#include <math/Stats.h>
+#include <diagnostic/CpuMeter.h>
+
+using namespace miosix;
+using namespace miosix::interfaces;
+
+/* SPI1 binding to the sensor */
+typedef BusSPI<1,spi1::mosi,spi1::miso,spi1::sck> busSPI1;
+typedef ProtocolSPI<busSPI1,sensors::lsm6ds3h::cs> spiLSM6DS3H0_a;
+typedef LSM6DS3H<spiLSM6DS3H0_a> lsm6ds3h_t;
+
+int main()
+{   
+    SimpleSensorSampler sampler;
+    spiLSM6DS3H0_a::init();
+
+    lsm6ds3h_t* lsm6ds3h = new lsm6ds3h_t(3,3);
+    
+    if(lsm6ds3h->init())
+    {
+        printf("[LSM6DS3H] Init succeeded\n" );
+        sampler.AddSensor(lsm6ds3h);
+    }
+    else
+    {
+        printf("[LSM6DS3H] Init failed\n");
+
+        while(!lsm6ds3h->init()) {
+            printf("[LSM6DS3H] Init failed\n");
+            Thread::sleep(1000);
+        }
+
+    }
+    
+
+    while(true)
+    {
+        sampler.Update();
+
+        // const Vec3* last_data = lsm6ds3h->gyroDataPtr();
+        // printf("%f %f %f\n", last_data->getX(), last_data->getY(),
+        //        last_data->getZ());
+
+        const Vec3* last_data = lsm6ds3h->accelDataPtr();
+        printf("%f %f %f\n", last_data->getX(), last_data->getY(),
+               last_data->getZ());
+        
+        // const float* last_temp = lsm6ds3h->tempDataPtr();
+        // printf("temp: %f\n", *last_temp);
+
+        Thread::sleep(100);
+    }
+}
\ No newline at end of file
diff --git a/src/tests/drivers/test-piksi.cpp b/src/tests/drivers/test-piksi.cpp
index c4a604f5190858001bcd8184c12f33112071ecaf..b4e01bc9e9c3983c7bcb311b4bbf29b12195000e 100644
--- a/src/tests/drivers/test-piksi.cpp
+++ b/src/tests/drivers/test-piksi.cpp
@@ -57,7 +57,7 @@ int main()
                  << " lon: " << gps.longitude << " h: " << gps.height
                  << " vn: " << gps.velocityNorth << " ve: " << gps.velocityEast
                  << " vd: " << gps.velocityDown << " ns: " << gps.numSatellites
-                 << endl;
+                 << " now: " << now;
         }
         catch (...)
         {
diff --git a/src/tests/misc/xbee-bitrate.cpp b/src/tests/misc/xbee-bitrate.cpp
index 7fbe56c59121279e97df6df67b1a083158339c41..229fa358f1f54b48fc16f47d6aeee8b5aaf8b239 100644
--- a/src/tests/misc/xbee-bitrate.cpp
+++ b/src/tests/misc/xbee-bitrate.cpp
@@ -95,7 +95,7 @@ bool sendPacket(uint8_t size)
     snd_buf[0] = '{';
     snd_buf[size-1] = '}';
 
-    
+
     for(int i = 0; i < size - 2; i++)
     {
         snd_buf[i+1] = ((snd_cntr + i) % 75 ) + 48; //ASCII char from 0 to z
@@ -145,7 +145,7 @@ int main()
             }
             results[i] = getTick() - start;
         }
-        printf("Results for %d byte packet size:\n");
+        printf("Results for %d byte packet size:\n", pkt_size);
         for(int i = 0; i < PKT_NUM; i++)
         {
             printf("%d\n", (int)results[i]);