diff --git a/sbs.conf b/sbs.conf
index b96726de49c219d4fd045584f4e5b555818d896a..90a0f358df82d1b239ffbe165d0614f66b83b9b7 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -120,6 +120,9 @@ Files:      src/shared/SensorManager/SensorManager.cpp
             src/boards/HeliTest/FlightModeManager/HeliFMM.cpp
             src/boards/HeliTest/ScreenManager.cpp
 
+[tests]
+Type:       srcfiles
+Files:      src/tests/catch/test-buttonhandler.cpp
 
 # Boards 
 [mxgui]
@@ -170,3 +173,11 @@ Include:	%logger %shared %heli-board %pwm
 Defines:    -DDEBUG
 Main:       heli-entry
 
+[tests-catch]
+Type:       test
+BoardId:    stm32f429zi_skyward_rogallina
+BinName:    tests-catch
+Include:	%tests %logger
+Defines:    -DDEBUG
+Main:       catch/catch-tests-entry
+
diff --git a/src/shared/ButtonHandler.h b/src/shared/ButtonHandler.h
index edd7c098d631cbf0ceda38319b90108bc18ae9a5..a68d86c887f7c0fa34881df86e03dfa0c951bf2d 100644
--- a/src/shared/ButtonHandler.h
+++ b/src/shared/ButtonHandler.h
@@ -24,8 +24,8 @@
 
 #include <functional>
 
-#include "ButtonStatus.h"
 #include "ActiveObject.h"
+#include "ButtonStatus.h"
 #include "logger/Logger.h"
 
 using miosix::Thread;
@@ -58,14 +58,15 @@ public:
     ButtonHandler(uint8_t btn_id, ButtonCallback on_press)
         : ButtonHandler(btn_id, on_press, on_press, on_press)
     {
-       
     }
 
-    ButtonHandler(uint8_t btn_id, ButtonCallback on_short_press, ButtonCallback on_long_press,
+    ButtonHandler(uint8_t btn_id, ButtonCallback on_short_press,
+                  ButtonCallback on_long_press,
                   ButtonCallback on_very_long_press)
         : on_short_press(on_short_press), on_long_press(on_long_press),
           on_very_long_press(on_very_long_press)
     {
+        memset(&status, 0, sizeof(ButtonStatus));
         status.btn_id = btn_id;
         Button::mode(miosix::Mode::INPUT);
     }
@@ -81,25 +82,36 @@ protected:
                 status.pressed_ticks++;
                 status.pressed   = true;
                 status.timestamp = miosix::getTick();
+
                 Logger::instance().log(status);
             }
-            else if (status.pressed)
+            else if (status.pressed)  // if the button was unpressed since the
+                                      // last operation
             {
                 if (status.pressed_ticks >= VERY_LONG_PRESS_TICKS)
                 {
-                    if(on_very_long_press){
+                    TRACE("Button pressed (very long) (%d ticks).\n",
+                          status.pressed_ticks);
+                    if (on_very_long_press)
+                    {
                         on_very_long_press(ButtonPress::VERY_LONG);
                     }
                 }
                 else if (status.pressed_ticks >= LONG_PRESS_TICKS)
                 {
-                    if(on_long_press){
+                    TRACE("Button pressed (long) (%d ticks).\n",
+                          status.pressed_ticks);
+                    if (on_long_press)
+                    {
                         on_long_press(ButtonPress::LONG);
                     }
                 }
                 else
                 {
-                    if(on_short_press){
+                    TRACE("Button pressed (short) (%d ticks).\n",
+                          status.pressed_ticks);
+                    if (on_short_press)
+                    {
                         on_short_press(ButtonPress::SHORT);
                     }
                 }
diff --git a/src/tests/catch/catch-tests-entry.cpp b/src/tests/catch/catch-tests-entry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e9d0fcc1c6dcf6ba08285d2dddf6b67a8a08eaa8
--- /dev/null
+++ b/src/tests/catch/catch-tests-entry.cpp
@@ -0,0 +1,155 @@
+/*
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+/*
+ * Entrypoint for all Catch1-based tests. You should not need to modify this
+ * file.
+ *
+ * To add a test, just create a new source file and include <catch.hpp>, then
+ * add test cases as you wish. Do not add a main() function in your test
+ * sources.
+ * Once the test is written, compile the test source file(s) + this entrypoint
+ * in sbs.conf. Run it and all your test will be automatically executed.
+ *
+ * You can also include this file in your test source if you want to run it as a
+ * standalone test, and not togheter with all the others. See the
+ * skyward-boardcore wiki at: <link here> for more information.
+ *
+ * You can specify command line options such as "-s" to display messages for
+ * succesfull tests (and not only messages for the ones that fail), plus others.
+ * You can also add tags in order to specify which tests you want to be
+ * executed. For a list of the command line arguments, take a look at:
+ * https://github.com/catchorg/Catch2/blob/Catch1.x/docs/command-line.md
+ *
+ * To specify the command line options, add, in the definition of the entrypoint
+ * in sbs.conf, add the CATCH1_CL_OPTIONS define.
+ * Example: -DCATCH1_CL_OPTIONS="\"[tag1][tag2] -s\""
+ * Remember to correctly escape the quotation marks, as shown
+ * above.
+ *
+ * Further information at:
+ * https://git.skywarder.eu/r2a/skyward-boardcore/wikis/Testing
+ */
+
+#define CATCH_CONFIG_RUNNER
+#define CATCH_CONFIG_NO_POSIX_SIGNALS
+
+#include <miosix.h>
+#include <utils/catch.hpp>
+#include <cstring>
+#include <string>
+#include <vector>
+
+using miosix::Thread;
+using std::string;
+using std::vector;
+
+// No tags or command line options as default.
+#ifndef CATCH1_CL_OPTIONS
+#define CATCH1_CL_OPTIONS ""
+#endif
+
+/**
+ * @brief Splits a string around spaces
+ * Eg: "This is a string" -> {"This", "is", "a", "string"}
+ *
+ * @param str String to split
+ * @return vector<string>
+ */
+vector<string> splitSpaces(string str)
+{
+    unsigned int p = 0, p2;
+    bool end       = false;
+    vector<string> out;
+
+    do
+    {
+        p2 = str.find(" ", p);
+
+        if (p2 == string::npos)  // No match
+        {
+            p2  = str.length();
+            end = true;
+        }
+
+        size_t len = p2 - p;  // Length of the substring
+
+        if (len > 0)
+        {
+            out.push_back(str.substr(p, len));
+        }
+        p = p2 + 1;
+    } while (!end);
+
+    return out;
+}
+
+/**
+ * @brief Entrypoint for the tests. Parses the options and tags
+ * provided in sbs.conf and runs a Catch1 session.
+ */
+int main()
+{
+    // Parse command line arguments from #defines in sbs.conf
+
+    string skw{"Skyward-tests"};
+
+    // CATCH1_CL_OPTIONS defined in sbs.conf
+    string options_str{CATCH1_CL_OPTIONS};
+
+    vector<string> options = splitSpaces(options_str);
+    vector<string> args{skw};
+
+    if (options.size() > 0)
+        args.insert(args.end(), options.begin(), options.end());
+
+    // Convert vector of strings to array of c-strings
+    size_t argc = args.size();
+
+    char** argv =
+        new char*[argc];  // Array of c strings, aka array of char pointers
+    for (size_t i = 0; i < argc; i++)
+    {
+        string s    = args.at(i);
+        char* c_arg = new char[s.length() + 1];
+        strcpy(c_arg, s.c_str());
+        argv[i] = c_arg;
+    }
+
+    // Run tests with the provided arguments
+    int result = Catch::Session().run(argc, argv);
+
+    // Clear memory
+    for (size_t i = 0; i < argc; i++)
+    {
+        delete[] argv[i];
+    }
+    delete[] argv;
+
+    printf("End.\n");
+    // Infinite loop to avoid board reset each time we return
+    for (;;)
+    {
+        Thread::sleep(10000);
+    }
+}
\ No newline at end of file
diff --git a/src/tests/catch/test-buttonhandler.cpp b/src/tests/catch/test-buttonhandler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa41d8e3d9e4b5ff28cb58cbcbb28a6b0dae1a95
--- /dev/null
+++ b/src/tests/catch/test-buttonhandler.cpp
@@ -0,0 +1,282 @@
+/*
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifdef STANDALONE_CATCH1_TEST
+#include "catch-tests-entry.cpp"
+#endif
+
+#include <functional>
+
+#include <Singleton.h>
+#include <miosix.h>
+#include <utils/catch.hpp>
+
+#include "ButtonHandler.h"
+
+using namespace miosix;
+using std::bind;
+
+class ButtonMock : public Singleton<ButtonMock>
+{
+    friend class Singleton<ButtonMock>;
+
+public:
+    // Mocked methods
+    static void mode(miosix::Mode::Mode_ mode)
+    {
+        getInstance()->input_mode = mode == Mode::INPUT;
+    }
+
+    static int value()
+    {
+        ButtonMock* instance = getInstance();
+        if (instance->input_mode)
+            return getInstance()->btn_value;
+
+        TRACE("Returning 0\n");
+        return 0;
+    }
+
+    // Test methods
+    void setValue(int value) { btn_value = value; }
+
+    void reset()
+    {
+        btn_value  = 0;
+        input_mode = false;
+    }
+
+    void press(unsigned int duration)
+    {
+        setValue(1);
+        Thread::sleep(duration);
+        setValue(0);
+        Thread::sleep(TICK_LENGTH * 2);
+    }
+
+private:
+    int btn_value   = 0;
+    bool input_mode = false;
+};
+
+ButtonMock* btn = ButtonMock::getInstance();
+
+using ButtonHandler_t = ButtonHandler<ButtonMock>;
+
+class ButtonHandlerTestFixture
+{
+
+public:
+    ButtonHandlerTestFixture() : handler(nullptr) {}
+
+    ~ButtonHandlerTestFixture()
+    {
+        if (handler)
+        {
+            handler->stop();
+            delete handler;
+        }
+
+        btn->reset();
+    }
+
+protected:
+    void registerOneCallback()
+    {
+        using namespace std::placeholders;
+        ButtonHandler_t::ButtonCallback cb =
+            bind(&ButtonHandlerTestFixture::singleCallback, this, _1);
+        handler = new ButtonHandler_t(1, cb);
+    }
+
+    void registerMultipleCallbacks()
+    {
+        using namespace std::placeholders;
+        ButtonHandler_t::ButtonCallback cb_short =
+            bind(&ButtonHandlerTestFixture::shortPressCallback, this, _1);
+        ButtonHandler_t::ButtonCallback cb_long =
+            bind(&ButtonHandlerTestFixture::longPressCallback, this, _1);
+        ButtonHandler_t::ButtonCallback cb_verylong =
+            bind(&ButtonHandlerTestFixture::veryLongPressCallback, this, _1);
+        handler = new ButtonHandler_t(1, cb_short, cb_long, cb_verylong);
+    }
+
+    void singleCallback(ButtonHandler_t::ButtonPress press_type)
+    {
+        switch (press_type)
+        {
+            case ButtonHandler_t::ButtonPress::SHORT:
+                ++short_press_count;
+                break;
+            case ButtonHandler_t::ButtonPress::LONG:
+                ++long_press_count;
+                break;
+            case ButtonHandler_t::ButtonPress::VERY_LONG:
+                ++very_long_press_count;
+                break;
+            default:
+                break;
+        }
+    }
+
+    void shortPressCallback(ButtonHandler_t::ButtonPress press_type)
+    {
+        UNUSED(press_type);
+        ++short_press_count;
+    }
+
+    void longPressCallback(ButtonHandler_t::ButtonPress press_type)
+    {
+        UNUSED(press_type);
+        ++long_press_count;
+    }
+
+    void veryLongPressCallback(ButtonHandler_t::ButtonPress press_type)
+    {
+        UNUSED(press_type);
+        ++very_long_press_count;
+    }
+
+    unsigned int short_press_count     = 0;
+    unsigned int long_press_count      = 0;
+    unsigned int very_long_press_count = 0;
+    ButtonHandler_t* handler;
+};
+
+TEST_CASE_METHOD(ButtonHandlerTestFixture,
+                 "ButtonHandler - Nothing happens if button is not pressed")
+{
+    registerMultipleCallbacks();
+    handler->start();
+
+    Thread::sleep((VERY_LONG_PRESS_TICKS + 1) * TICK_LENGTH);
+
+    REQUIRE(short_press_count == 0);
+    REQUIRE(long_press_count == 0);
+    REQUIRE(very_long_press_count == 0);
+}
+
+TEST_CASE_METHOD(ButtonHandlerTestFixture,
+                 "ButtonHandler - Handle each button press")
+{
+    SECTION("Different callbacks")
+    {
+        registerMultipleCallbacks();
+        handler->start();
+
+        btn->press(TICK_LENGTH + 5);
+
+        
+        CHECK(long_press_count == 0);
+        CHECK(very_long_press_count == 0);
+        REQUIRE(short_press_count == 1);
+
+        btn->press(TICK_LENGTH * (LONG_PRESS_TICKS + 1));
+
+        CHECK(short_press_count == 1);
+        CHECK(very_long_press_count == 0);
+        REQUIRE(long_press_count == 1);
+        
+
+        btn->press(TICK_LENGTH * (VERY_LONG_PRESS_TICKS + 1));
+
+        CHECK(short_press_count == 1);
+        CHECK(long_press_count == 1);
+        REQUIRE(very_long_press_count == 1);
+    }
+
+    SECTION("Same callback")
+    {
+        registerOneCallback();
+        handler->start();
+
+        btn->press(TICK_LENGTH + 5);
+  
+        CHECK(long_press_count == 0);
+        CHECK(very_long_press_count == 0);
+        REQUIRE(short_press_count == 1);
+
+        btn->press(TICK_LENGTH * (LONG_PRESS_TICKS + 1));
+
+        CHECK(short_press_count == 1); 
+        CHECK(very_long_press_count == 0);
+        REQUIRE(long_press_count == 1);
+
+        btn->press(TICK_LENGTH * (VERY_LONG_PRESS_TICKS + 1));
+
+
+        CHECK(short_press_count == 1);
+        CHECK(long_press_count == 1);
+        REQUIRE(very_long_press_count == 1);
+    }
+}
+
+TEST_CASE_METHOD(ButtonHandlerTestFixture,
+                 "ButtonHandler - Just short of each button press")
+{
+    SECTION("Different callbacks")
+    {
+        registerMultipleCallbacks();
+        handler->start();
+        
+        REQUIRE(long_press_count == 0);
+        REQUIRE(very_long_press_count == 0);
+        REQUIRE(short_press_count == 0);
+
+        btn->press(TICK_LENGTH * (LONG_PRESS_TICKS - 1));
+
+        REQUIRE(short_press_count == 1);
+        REQUIRE(very_long_press_count == 0);
+        REQUIRE(long_press_count == 0);
+        
+
+        btn->press(TICK_LENGTH * (VERY_LONG_PRESS_TICKS - 1));
+
+        REQUIRE(short_press_count == 1);
+        REQUIRE(long_press_count == 1);
+        REQUIRE(very_long_press_count == 0);
+    }
+
+    SECTION("Same callback")
+    {
+        registerOneCallback();
+        handler->start();
+   
+        REQUIRE(long_press_count == 0);
+        REQUIRE(very_long_press_count == 0);
+        REQUIRE(short_press_count == 0);
+
+        btn->press(TICK_LENGTH * (LONG_PRESS_TICKS - 1));
+
+        REQUIRE(short_press_count == 1);
+        REQUIRE(very_long_press_count == 0);
+        REQUIRE(long_press_count == 0);
+        
+
+        btn->press(TICK_LENGTH * (VERY_LONG_PRESS_TICKS - 1));
+
+        REQUIRE(short_press_count == 1);
+        REQUIRE(long_press_count == 1);
+        REQUIRE(very_long_press_count == 0);
+    }
+}
\ No newline at end of file