diff --git a/src/boards/Ciuti/Algorithm/UprightDetector.cpp b/src/boards/Ciuti/Algorithm/UprightDetector.cpp
index bf62f453f3b379c6d496cbc7d8cb972cdb24157c..ab1649ea8f33e2e995bf1e50cf713d2f0279058f 100644
--- a/src/boards/Ciuti/Algorithm/UprightDetector.cpp
+++ b/src/boards/Ciuti/Algorithm/UprightDetector.cpp
@@ -31,6 +31,7 @@
 
 /*
 % Matlab algorithm
+% Author: Marco Marchesi
 testlog = ciutilog;
 testlog.timestamp = testlog.timestamp*1e-6;
 angle_trigger = deg2rad(75);
@@ -59,27 +60,12 @@ for i = 1:length(z_filter)
 end
 */
 
-namespace Ciuti
-{
-
-void UprightDetector::start()
-{
-    BoardScheduler::getInstance().getScheduler().addTask(
-        [=]() { this->update(); }, UprightDetectorConfig::ALGO_PERIOD);
-}
+using namespace Boardcore;
 
-void UprightDetector::update()
+namespace Ciuti
 {
-    auto sample = Sensors::getInstance().getLIS331HHLastSample();
-
-    if (!triggered)
-    {
-        algoStep(sample.accelerationZ -
-                 Ciuti::SensorsConfig::Z_AXIS_OFFSET_LIS331HH);
-    }
-}
 
-void UprightDetector::algoStep(float axis)
+void UprightDetector::update(float axis)
 {
     filtered.push(axis);
 
@@ -92,17 +78,36 @@ void UprightDetector::algoStep(float axis)
         count = 0;
     }
 
-    if (count > UprightDetectorConfig::DETECT_SAMPLES && !triggered)
-    {
-        triggered = true;
-        trigger();
+    upright = count > UprightDetectorConfig::DETECT_SAMPLES;
+}
+
+void UprightDetectorController::start()
+{
+    BoardScheduler::getInstance().getScheduler().addTask(
+        [=]() { this->update(); }, UprightDetectorConfig::ALGO_PERIOD);
+}
+
+void UprightDetectorController::update()
+{
+    auto sample = Sensors::getInstance().getLIS331HHLastSample();
+
+    if(!fired) {
+        algo.update(sample.accelerationZ -
+                 Ciuti::SensorsConfig::Z_AXIS_OFFSET_LIS331HH);
+
+        if(algo.isUpright()) {
+            trigger();
+            fired = true;
+        }
     }
 }
 
-void UprightDetector::trigger()
+void UprightDetectorController::trigger()
 {
     LOG_INFO(logger, "Upright triggered!");
-    // TODO:
+
+    Logger::getInstance().resetStats();
+    Logger::getInstance().start();
 }
 
 }  // namespace Ciuti
diff --git a/src/boards/Ciuti/Algorithm/UprightDetector.h b/src/boards/Ciuti/Algorithm/UprightDetector.h
index af754211487539f18d22047b7d75837330f02a28..ba96e58363d330601b785b7f88579e60e97e68b4 100644
--- a/src/boards/Ciuti/Algorithm/UprightDetector.h
+++ b/src/boards/Ciuti/Algorithm/UprightDetector.h
@@ -32,25 +32,38 @@
 namespace Ciuti
 {
 
-class UprightDetector : public Boardcore::Singleton<UprightDetector>
+class UprightDetector
 {
-    friend Boardcore::Singleton<UprightDetector>;
-
 public:
     UprightDetector() {}
 
+    void update(float axis);
+    bool isUpright() { return upright; }
+
+private:
+    bool upright = false;
+    int count    = 0;
+    Boardcore::MovingAverage<float, UprightDetectorConfig::MEAN_SAMPLES>
+        filtered;
+};
+
+class UprightDetectorController
+    : public Boardcore::Singleton<UprightDetectorController>
+{
+    friend class Boardcore::Singleton<UprightDetectorController>;
+
+public:
     void start();
 
 private:
-    void update();
+    UprightDetectorController() {}
 
+    void update();
     void trigger();
-    void algoStep(float axis);
 
-    bool triggered = false;
-    int count      = 0;
-    Boardcore::MovingAverage<float, UprightDetectorConfig::MEAN_SAMPLES>
-        filtered;
+    bool fired = false;
+
+    UprightDetector algo;
 
     Boardcore::PrintLogger logger =
         Boardcore::Logging::getLogger("ciuti.uprightdetector");
diff --git a/src/boards/Ciuti/Configs/SensorsConfig.h b/src/boards/Ciuti/Configs/SensorsConfig.h
index 0610aa0d5fb0f56387468bad7332f6ee0237a2fb..0699f8d43d7bc28dd709c8085484444787ecbaad 100644
--- a/src/boards/Ciuti/Configs/SensorsConfig.h
+++ b/src/boards/Ciuti/Configs/SensorsConfig.h
@@ -36,7 +36,7 @@ constexpr Boardcore::InternalADC::Channel INTERNAL_ADC_CH_0 =
     Boardcore::InternalADC::Channel::CH0;
 constexpr Boardcore::InternalADC::Channel INTERNAL_ADC_CH_1 =
     Boardcore::InternalADC::Channel::CH1;
-constexpr unsigned int SAMPLE_PERIOD_INTERNAL_ADC = 1;
+constexpr unsigned int SAMPLE_PERIOD_INTERNAL_ADC = 200;
 
 // LIS331HH
 constexpr unsigned int SAMPLE_PERIOD_LIS331HH = 1000 / 50;
diff --git a/src/boards/Ciuti/Serial/SerialWatcher.cpp b/src/boards/Ciuti/Serial/SerialWatcher.cpp
index cd1648b251b52c44aed2418189e7e48d53ac2efe..4b648d1446edd3b2fcb8ea6af67c056d8ff706ed 100644
--- a/src/boards/Ciuti/Serial/SerialWatcher.cpp
+++ b/src/boards/Ciuti/Serial/SerialWatcher.cpp
@@ -22,25 +22,57 @@
 
 #include "SerialWatcher.h"
 
-#include <utils/Debug.h>
+#include <Ciuti/BoardScheduler.h>
 #include <Ciuti/Buses.h>
+#include <drivers/timer/TimestampTimer.h>
 #include <miosix.h>
+#include <utils/Debug.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+// Simple xorshift RNG
+uint32_t xorshift32()
+{
+    // https://xkcd.com/221/
+    static uint32_t STATE = 0x08104444;
+
+    uint32_t x = STATE;
+    x ^= x << 13;
+    x ^= x >> 17;
+    x ^= x << 5;
+
+    // Do I really need to lock this? Nah, introduce extra randomness.
+    return STATE = x;
+}
 
 namespace Ciuti
 {
 
+SerialWatcher::SerialWatcher(USART &usart, unsigned int id) : usart(usart)
+{
+    stats.usart_id = id;
+}
+
 void SerialWatcher::run()
 {
-    while(isRunning()) {
-        uint8_t sent = 25;
-        uint8_t recv;
+    while (isRunning())
+    {
+        long long now = getTick();
+
+        uint32_t sent = xorshift32();
+        uint32_t recv;
 
+        // Perform a loopback read
         usart.clearQueue();
-        usart.write(&sent, 1);
+        usart.write(&sent, 4);
+        usart.read(&recv, 4);
 
-        usart.read(&recv, 1);
+        if (sent != recv)
+            stats.error_count += 1;
 
-        miosix::Thread::sleep(1000);
+        stats.last_timestamp = TimestampTimer::getTimestamp();
+        Thread::sleepUntil(now + SerialWatcher::PERIOD);
     }
 }
 
@@ -48,24 +80,28 @@ void SerialWatcherController::start()
 {
     serial_watcher1->start();
     serial_watcher2->start();
-}
 
-void SerialWatcherController::stop()
-{
-    serial_watcher1->stop();
-    serial_watcher2->stop();
+    BoardScheduler::getInstance().getScheduler().addTask(
+        [=]() { Logger::getInstance().log(this->serial_watcher1->getStats()); },
+        SerialWatcher::PERIOD);
+
+    BoardScheduler::getInstance().getScheduler().addTask(
+        [=]() { Logger::getInstance().log(this->serial_watcher2->getStats()); },
+        SerialWatcher::PERIOD);
+
+    LOG_INFO(logger, "Serial watcher controller setup done!");
 }
 
-SerialWatcherController::SerialWatcherController() 
+SerialWatcherController::SerialWatcherController()
 {
-    serial_watcher1 = new SerialWatcher(Ciuti::Buses::getInstance().usart2);
-    serial_watcher2 = new SerialWatcher(Ciuti::Buses::getInstance().usart3);
+    serial_watcher1 = new SerialWatcher(Ciuti::Buses::getInstance().usart2, 2);
+    serial_watcher2 = new SerialWatcher(Ciuti::Buses::getInstance().usart3, 3);
 }
 
-SerialWatcherController::~SerialWatcherController() 
+SerialWatcherController::~SerialWatcherController()
 {
     delete serial_watcher1;
     delete serial_watcher2;
 }
 
-}
\ No newline at end of file
+}  // namespace Ciuti
\ No newline at end of file
diff --git a/src/boards/Ciuti/Serial/SerialWatcher.h b/src/boards/Ciuti/Serial/SerialWatcher.h
index 837cc4c3df897acbdd6572a1b02bb28cfb689cfe..dbb3d2961a5845c1a6f354f093891111e7a0363a 100644
--- a/src/boards/Ciuti/Serial/SerialWatcher.h
+++ b/src/boards/Ciuti/Serial/SerialWatcher.h
@@ -25,6 +25,9 @@
 #include <ActiveObject.h>
 #include <Singleton.h>
 #include <drivers/usart/USART.h>
+#include <diagnostic/PrintLogger.h>
+
+#include "SerialWatcherStats.h"
 
 namespace Ciuti
 {
@@ -32,11 +35,17 @@ namespace Ciuti
 class SerialWatcher : public Boardcore::ActiveObject
 {
 public:
-    SerialWatcher(Boardcore::USART &usart) : usart(usart) {}
+    static constexpr long long PERIOD = 1000;
+
+    SerialWatcher(Boardcore::USART &usart, unsigned int id);
+
+    SerialWatcherStats getStats() { return stats; }
 
 private:
     void run() override;
 
+    SerialWatcherStats stats;
+
     Boardcore::USART &usart;
 };
 
@@ -47,7 +56,6 @@ class SerialWatcherController
 
 public:
     void start();
-    void stop();
 
 private:
     SerialWatcherController();
@@ -55,6 +63,9 @@ private:
 
     SerialWatcher *serial_watcher1 = nullptr;
     SerialWatcher *serial_watcher2 = nullptr;
+
+    Boardcore::PrintLogger logger =
+        Boardcore::Logging::getLogger("ciuti.serialwatcher");
 };
 
 }  // namespace Ciuti
\ No newline at end of file
diff --git a/src/boards/Ciuti/Serial/SerialWatcherStats.h b/src/boards/Ciuti/Serial/SerialWatcherStats.h
new file mode 100644
index 0000000000000000000000000000000000000000..9f083fd2d30eeb2ecc5c1b1f6c87e4bdde51e6e0
--- /dev/null
+++ b/src/boards/Ciuti/Serial/SerialWatcherStats.h
@@ -0,0 +1,37 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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 <cstdint>
+
+namespace Ciuti 
+{
+
+struct SerialWatcherStats
+{
+    uint64_t last_timestamp = 0;
+    unsigned int error_count = 0;
+    unsigned int usart_id = 0;
+};
+
+}
\ No newline at end of file
diff --git a/src/entrypoints/Ciuti/ciuti-entry.cpp b/src/entrypoints/Ciuti/ciuti-entry.cpp
index a6e2494025d084408f91167ca090920c0988dbe9..9b75822c5956614f64ee786d33bcdf099aa981c5 100644
--- a/src/entrypoints/Ciuti/ciuti-entry.cpp
+++ b/src/entrypoints/Ciuti/ciuti-entry.cpp
@@ -32,22 +32,42 @@ using namespace miosix;
 using namespace Boardcore;
 using namespace Ciuti;
 
+void printCiutiBanner()
+{
+    printf(
+        R"( Booting ...                                 )""\n"
+        R"( ________  ___  ___  ___  _________  ___     )""\n"
+        R"(|\   ____\|\  \|\  \|\  \|\___   ___\\  \    )""\n"
+        R"(\ \  \___|\ \  \ \  \\\  \|___ \  \_\ \  \   )""\n"
+        R"( \ \  \    \ \  \ \  \\\  \   \ \  \ \ \  \  )""\n"
+        R"(  \ \  \____\ \  \ \  \\\  \   \ \  \ \ \  \ )""\n"
+        R"(   \ \_______\ \__\ \_______\   \ \__\ \ \__\)""\n"
+        R"(    \|_______|\|__|\|_______|    \|__|  \|__|)""\n"
+        R"(                     On Board Software       )""\n"
+    );
+}
+
 int main()
 {
-    // Logger::getInstance().start();
+    printCiutiBanner();
+
+    Boardcore::PrintLogger logger =
+        Boardcore::Logging::getLogger("ciuti.main");
+
     Sensors::getInstance().start();
-    UprightDetector::getInstance().start();
+    UprightDetectorController::getInstance().start();
     SerialWatcherController::getInstance().start();
 
     BoardScheduler::getInstance().getScheduler().start();
 
+    LOG_INFO(logger, "Initialization complete!");
+
     // Periodical statistics
     while (true)
     {
         Thread::sleep(1000);
-        // Logger::getInstance().log(CpuMeter::getCpuStats());
-        // CpuMeter::resetCpuStats();
-        // Logger::getInstance().logStats();
-        // MemoryProfiling::print();
+        Logger::getInstance().log(CpuMeter::getCpuStats());
+        CpuMeter::resetCpuStats();
+        Logger::getInstance().logStats();
     }
 }