From 78bea10653b656812fbe86fac2d488e3b15a0f44 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Tue, 13 Sep 2022 23:32:03 +0200
Subject: [PATCH] [Ciuti] Initial CIUTI implementation

---
 CMakeLists.txt                                |   4 +
 cmake/dependencies.cmake                      |   2 +
 .../Ciuti/Algorithm/UprightDetector.cpp       | 108 ++++++++++++++++++
 src/boards/Ciuti/Algorithm/UprightDetector.h  |  59 ++++++++++
 .../Ciuti/Algorithm/UprightDetectorConfig.h   |  47 ++++++++
 src/boards/Ciuti/Buses.h                      |  54 +++++++++
 src/boards/Ciuti/Configs/SensorsConfig.h      |   8 +-
 src/boards/Ciuti/Sensors/Sensors.cpp          |  38 +++++-
 src/boards/Ciuti/Sensors/Sensors.h            |   6 +-
 src/boards/Ciuti/Serial/SerialWatcher.cpp     |  71 ++++++++++++
 src/boards/Ciuti/Serial/SerialWatcher.h       |  60 ++++++++++
 src/entrypoints/Ciuti/ciuti-entry.cpp         |  26 ++---
 12 files changed, 460 insertions(+), 23 deletions(-)
 create mode 100644 src/boards/Ciuti/Algorithm/UprightDetector.cpp
 create mode 100644 src/boards/Ciuti/Algorithm/UprightDetector.h
 create mode 100644 src/boards/Ciuti/Algorithm/UprightDetectorConfig.h
 create mode 100644 src/boards/Ciuti/Buses.h
 create mode 100644 src/boards/Ciuti/Serial/SerialWatcher.cpp
 create mode 100644 src/boards/Ciuti/Serial/SerialWatcher.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 239717931..363e06779 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -71,6 +71,10 @@ add_executable(payload-entry src/entrypoints/Payload/payload-entry.cpp ${PAYLOAD
 target_include_directories(payload-entry PRIVATE ${OBSW_INCLUDE_DIRS})
 sbs_target(payload-entry stm32f429zi_skyward_death_stack_x)
 
+add_executable(ciuti-entry src/entrypoints/Ciuti/ciuti-entry.cpp ${CIUTI_COMPUTER})
+target_include_directories(ciuti-entry PRIVATE ${OBSW_INCLUDE_DIRS})
+sbs_target(ciuti-entry stm32f205rc_skyward_ciuti)
+
 add_executable(groundstation-entry src/entrypoints/Groundstation/groundstation-entry.cpp)
 sbs_target(groundstation-entry stm32f429zi_skyward_groundstation_v2)
 
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index bc8017724..d99e2d6cc 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -82,6 +82,8 @@ set(PARAFOIL_COMPUTER
 
 set(CIUTI_COMPUTER
     src/boards/Ciuti/Sensors/Sensors.cpp
+    src/boards/Ciuti/Algorithm/UprightDetector.cpp
+    src/boards/Ciuti/Serial/SerialWatcher.cpp
 )
 
 set(MOCK_LOGGER skyward-boardcore/src/shared/mock-Logger.cpp)
diff --git a/src/boards/Ciuti/Algorithm/UprightDetector.cpp b/src/boards/Ciuti/Algorithm/UprightDetector.cpp
new file mode 100644
index 000000000..bf62f453f
--- /dev/null
+++ b/src/boards/Ciuti/Algorithm/UprightDetector.cpp
@@ -0,0 +1,108 @@
+/* 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.
+ */
+
+#include "UprightDetector.h"
+
+#include <Ciuti/BoardScheduler.h>
+#include <Ciuti/Configs/SensorsConfig.h>
+#include <Ciuti/Sensors/Sensors.h>
+#include <utils/Debug.h>
+
+#include <cmath>
+
+/*
+% Matlab algorithm
+testlog = ciutilog;
+testlog.timestamp = testlog.timestamp*1e-6;
+angle_trigger = deg2rad(75);
+% maybe transform in radians% flags
+flagDetect = false;
+counter = 0;
+%
+detectTime = 5; %[s]
+sensorFrequency = 50; %[Hz]
+N_detectSamples = detectTime * sensorFrequency;
+z_filter = movmean(testlog.z,50);
+maxZ = max(abs(z_filter));
+offset = 0.20;
+z_filter = z_filter + offset;
+threshold = sin(angle_trigger);
+for i = 1:length(z_filter)
+    if abs(z_filter(i)) > threshold
+        counter = counter + 1;
+    else
+        counter = 0;
+    end
+    if counter > N_detectSamples
+        flagDetect = true;
+    end
+    detection_vec(i) = flagDetect;
+end
+*/
+
+namespace Ciuti
+{
+
+void UprightDetector::start()
+{
+    BoardScheduler::getInstance().getScheduler().addTask(
+        [=]() { this->update(); }, UprightDetectorConfig::ALGO_PERIOD);
+}
+
+void UprightDetector::update()
+{
+    auto sample = Sensors::getInstance().getLIS331HHLastSample();
+
+    if (!triggered)
+    {
+        algoStep(sample.accelerationZ -
+                 Ciuti::SensorsConfig::Z_AXIS_OFFSET_LIS331HH);
+    }
+}
+
+void UprightDetector::algoStep(float axis)
+{
+    filtered.push(axis);
+
+    if (std::abs(filtered.getAverage()) > UprightDetectorConfig::THRESHOLD)
+    {
+        count++;
+    }
+    else
+    {
+        count = 0;
+    }
+
+    if (count > UprightDetectorConfig::DETECT_SAMPLES && !triggered)
+    {
+        triggered = true;
+        trigger();
+    }
+}
+
+void UprightDetector::trigger()
+{
+    LOG_INFO(logger, "Upright triggered!");
+    // TODO:
+}
+
+}  // namespace Ciuti
diff --git a/src/boards/Ciuti/Algorithm/UprightDetector.h b/src/boards/Ciuti/Algorithm/UprightDetector.h
new file mode 100644
index 000000000..af7542114
--- /dev/null
+++ b/src/boards/Ciuti/Algorithm/UprightDetector.h
@@ -0,0 +1,59 @@
+/* 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 <Singleton.h>
+#include <diagnostic/PrintLogger.h>
+#include <sensors/LIS331HH/LIS331HHData.h>
+#include <utils/MovingAverage.h>
+
+#include "UprightDetectorConfig.h"
+
+namespace Ciuti
+{
+
+class UprightDetector : public Boardcore::Singleton<UprightDetector>
+{
+    friend Boardcore::Singleton<UprightDetector>;
+
+public:
+    UprightDetector() {}
+
+    void start();
+
+private:
+    void update();
+
+    void trigger();
+    void algoStep(float axis);
+
+    bool triggered = false;
+    int count      = 0;
+    Boardcore::MovingAverage<float, UprightDetectorConfig::MEAN_SAMPLES>
+        filtered;
+
+    Boardcore::PrintLogger logger =
+        Boardcore::Logging::getLogger("ciuti.uprightdetector");
+};
+
+}  // namespace Ciuti
\ No newline at end of file
diff --git a/src/boards/Ciuti/Algorithm/UprightDetectorConfig.h b/src/boards/Ciuti/Algorithm/UprightDetectorConfig.h
new file mode 100644
index 000000000..a46c76dce
--- /dev/null
+++ b/src/boards/Ciuti/Algorithm/UprightDetectorConfig.h
@@ -0,0 +1,47 @@
+/* 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 <cmath>
+
+namespace Ciuti
+{
+
+namespace UprightDetectorConfig
+{
+
+constexpr int ALGO_FREQUENCY        = 50;
+constexpr int DETECT_TIME           = 5;
+constexpr int MEAN_SAMPLES          = 50;
+constexpr float DETECT_ANGLE        = 80;
+constexpr float THRESHOLD_TOLERANCE = 0.1;
+
+constexpr int ALGO_PERIOD    = 1000 / 50;
+constexpr int DETECT_SAMPLES = DETECT_TIME * ALGO_FREQUENCY;
+
+constexpr float THRESHOLD =
+    std::sin(DETECT_ANGLE * M_PI / 180.0) * (1.0 - THRESHOLD_TOLERANCE);
+
+}  // namespace UprightDetectorConfig
+
+}  // namespace Ciuti
\ No newline at end of file
diff --git a/src/boards/Ciuti/Buses.h b/src/boards/Ciuti/Buses.h
new file mode 100644
index 000000000..6d46e09da
--- /dev/null
+++ b/src/boards/Ciuti/Buses.h
@@ -0,0 +1,54 @@
+/* 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 <Singleton.h>
+#include <drivers/spi/SPIBus.h>
+#include <drivers/usart/USART.h>
+#include <miosix.h>
+
+namespace Ciuti
+{
+
+struct Buses : public Boardcore::Singleton<Buses>
+{
+    friend class Boardcore::Singleton<Buses>;
+
+    Boardcore::USART usart2;
+    Boardcore::USART usart3;
+
+    Boardcore::SPIBus spi2;
+
+private:
+    Buses()
+        : usart2(USART2, Boardcore::USARTInterface::Baudrate::B115200),
+          usart3(USART3, Boardcore::USARTInterface::Baudrate::B115200),
+          spi2(SPI2)
+    {
+
+        usart2.init();
+        usart3.init();
+    }
+};
+
+}  // namespace Ciuti
\ No newline at end of file
diff --git a/src/boards/Ciuti/Configs/SensorsConfig.h b/src/boards/Ciuti/Configs/SensorsConfig.h
index 9bb4fb73d..0610aa0d5 100644
--- a/src/boards/Ciuti/Configs/SensorsConfig.h
+++ b/src/boards/Ciuti/Configs/SensorsConfig.h
@@ -24,7 +24,7 @@
 
 #include <drivers/adc/InternalADC.h>
 
-namespace Main
+namespace Ciuti
 {
 
 namespace SensorsConfig
@@ -38,6 +38,10 @@ constexpr Boardcore::InternalADC::Channel INTERNAL_ADC_CH_1 =
     Boardcore::InternalADC::Channel::CH1;
 constexpr unsigned int SAMPLE_PERIOD_INTERNAL_ADC = 1;
 
+// LIS331HH
+constexpr unsigned int SAMPLE_PERIOD_LIS331HH = 1000 / 50;
+constexpr float Z_AXIS_OFFSET_LIS331HH        = 0.2;
+
 }  // namespace SensorsConfig
 
-}  // namespace Main
+}  // namespace Ciuti
diff --git a/src/boards/Ciuti/Sensors/Sensors.cpp b/src/boards/Ciuti/Sensors/Sensors.cpp
index 54142d4d2..6b2b93e52 100644
--- a/src/boards/Ciuti/Sensors/Sensors.cpp
+++ b/src/boards/Ciuti/Sensors/Sensors.cpp
@@ -22,11 +22,12 @@
 
 #include "Sensors.h"
 
+#include <Ciuti/Buses.h>
 #include <Ciuti/Configs/SensorsConfig.h>
 
 using namespace miosix;
 using namespace Boardcore;
-using namespace Main::SensorsConfig;
+using namespace Ciuti::SensorsConfig;
 
 namespace Ciuti
 {
@@ -40,15 +41,26 @@ Boardcore::InternalADCData Sensors::getInternalADCLastSample(
     return internalAdc->getVoltage(channel);
 }
 
+Boardcore::LIS331HHData Sensors::getLIS331HHLastSample()
+{
+    PauseKernelLock lock;
+    return lis331hh->getLastSample();
+}
+
 Sensors::Sensors()
 {
     internalAdcInit();
+    lis331hhInit();
 
     // Create the sensor manager
     sensorManager = new SensorManager(sensorsMap);
 }
 
-Sensors::~Sensors() {}
+Sensors::~Sensors()
+{
+    delete internalAdc;
+    delete lis331hh;
+}
 
 void Sensors::internalAdcInit()
 {
@@ -58,7 +70,7 @@ void Sensors::internalAdcInit()
     internalAdc->enableChannel(INTERNAL_ADC_CH_1);
 
     SensorInfo info("INTERNAL_ADC", SAMPLE_PERIOD_INTERNAL_ADC,
-                    [&]()
+                    [=]()
                     {
                         Logger::getInstance().log(
                             internalAdc->getVoltage(INTERNAL_ADC_CH_0));
@@ -71,4 +83,24 @@ void Sensors::internalAdcInit()
     LOG_INFO(logger, "Internal ADC setup done!");
 }
 
+void Sensors::lis331hhInit()
+{
+    SPIBusConfig config;
+
+    lis331hh = new LIS331HH(Buses::getInstance().spi2,
+                            devices::lis331hh::cs::getPin(), config);
+
+    lis331hh->init();
+    lis331hh->setOutputDataRate(LIS331HH::ODR_50);
+    lis331hh->setFullScaleRange(LIS331HH::FS_24);
+
+    SensorInfo info("LIS331HH", SAMPLE_PERIOD_LIS331HH,
+                    [=]()
+                    { Logger::getInstance().log(lis331hh->getLastSample()); });
+
+    sensorsMap.emplace(std::make_pair(lis331hh, info));
+
+    LOG_INFO(logger, "LIS331HH setup done!");
+}
+
 }  // namespace Ciuti
\ No newline at end of file
diff --git a/src/boards/Ciuti/Sensors/Sensors.h b/src/boards/Ciuti/Sensors/Sensors.h
index b9daa1a09..828d3888e 100644
--- a/src/boards/Ciuti/Sensors/Sensors.h
+++ b/src/boards/Ciuti/Sensors/Sensors.h
@@ -25,6 +25,7 @@
 #include <Singleton.h>
 #include <diagnostic/PrintLogger.h>
 #include <drivers/adc/InternalADC.h>
+#include <sensors/LIS331HH/LIS331HH.h>
 #include <sensors/SensorManager.h>
 
 namespace Ciuti
@@ -40,14 +41,17 @@ public:
     Boardcore::InternalADCData getInternalADCLastSample(
         Boardcore::InternalADC::Channel channel);
 
+    Boardcore::LIS331HHData getLIS331HHLastSample();
+
 private:
     Sensors();
-
     ~Sensors();
 
     void internalAdcInit();
+    void lis331hhInit();
 
     Boardcore::InternalADC *internalAdc = nullptr;
+    Boardcore::LIS331HH *lis331hh       = nullptr;
 
     Boardcore::SensorManager *sensorManager = nullptr;
 
diff --git a/src/boards/Ciuti/Serial/SerialWatcher.cpp b/src/boards/Ciuti/Serial/SerialWatcher.cpp
new file mode 100644
index 000000000..cd1648b25
--- /dev/null
+++ b/src/boards/Ciuti/Serial/SerialWatcher.cpp
@@ -0,0 +1,71 @@
+/* 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.
+ */
+
+#include "SerialWatcher.h"
+
+#include <utils/Debug.h>
+#include <Ciuti/Buses.h>
+#include <miosix.h>
+
+namespace Ciuti
+{
+
+void SerialWatcher::run()
+{
+    while(isRunning()) {
+        uint8_t sent = 25;
+        uint8_t recv;
+
+        usart.clearQueue();
+        usart.write(&sent, 1);
+
+        usart.read(&recv, 1);
+
+        miosix::Thread::sleep(1000);
+    }
+}
+
+void SerialWatcherController::start()
+{
+    serial_watcher1->start();
+    serial_watcher2->start();
+}
+
+void SerialWatcherController::stop()
+{
+    serial_watcher1->stop();
+    serial_watcher2->stop();
+}
+
+SerialWatcherController::SerialWatcherController() 
+{
+    serial_watcher1 = new SerialWatcher(Ciuti::Buses::getInstance().usart2);
+    serial_watcher2 = new SerialWatcher(Ciuti::Buses::getInstance().usart3);
+}
+
+SerialWatcherController::~SerialWatcherController() 
+{
+    delete serial_watcher1;
+    delete serial_watcher2;
+}
+
+}
\ No newline at end of file
diff --git a/src/boards/Ciuti/Serial/SerialWatcher.h b/src/boards/Ciuti/Serial/SerialWatcher.h
new file mode 100644
index 000000000..837cc4c3d
--- /dev/null
+++ b/src/boards/Ciuti/Serial/SerialWatcher.h
@@ -0,0 +1,60 @@
+/* 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 <ActiveObject.h>
+#include <Singleton.h>
+#include <drivers/usart/USART.h>
+
+namespace Ciuti
+{
+
+class SerialWatcher : public Boardcore::ActiveObject
+{
+public:
+    SerialWatcher(Boardcore::USART &usart) : usart(usart) {}
+
+private:
+    void run() override;
+
+    Boardcore::USART &usart;
+};
+
+class SerialWatcherController
+    : public Boardcore::Singleton<SerialWatcherController>
+{
+    friend class Boardcore::Singleton<SerialWatcherController>;
+
+public:
+    void start();
+    void stop();
+
+private:
+    SerialWatcherController();
+    ~SerialWatcherController();
+
+    SerialWatcher *serial_watcher1 = nullptr;
+    SerialWatcher *serial_watcher2 = nullptr;
+};
+
+}  // namespace Ciuti
\ No newline at end of file
diff --git a/src/entrypoints/Ciuti/ciuti-entry.cpp b/src/entrypoints/Ciuti/ciuti-entry.cpp
index 0fe07e8c9..a6e249402 100644
--- a/src/entrypoints/Ciuti/ciuti-entry.cpp
+++ b/src/entrypoints/Ciuti/ciuti-entry.cpp
@@ -23,6 +23,8 @@
 #include <Ciuti/BoardScheduler.h>
 #include <Ciuti/Sensors/Sensors.h>
 #include <diagnostic/CpuMeter/CpuMeter.h>
+#include <Ciuti/Algorithm/UprightDetector.h>
+#include <Ciuti/Serial/SerialWatcher.h>
 
 #include <thread>
 
@@ -30,32 +32,22 @@ using namespace miosix;
 using namespace Boardcore;
 using namespace Ciuti;
 
-void print()
-{
-    auto ch0 =
-        Sensors::getInstance().getInternalADCLastSample(InternalADC::CH0);
-    auto ch1 =
-        Sensors::getInstance().getInternalADCLastSample(InternalADC::CH1);
-
-    printf("[%.2f] CH0: %.6f, CH1: %.6f, log number: %d\n",
-           ch0.voltageTimestamp / 1e6, ch0.voltage, ch1.voltage,
-           Logger::getInstance().getCurrentLogNumber());
-}
-
 int main()
 {
-    Logger::getInstance().start();
+    // Logger::getInstance().start();
     Sensors::getInstance().start();
+    UprightDetector::getInstance().start();
+    SerialWatcherController::getInstance().start();
 
-    BoardScheduler::getInstance().getScheduler().addTask(print, 100);
     BoardScheduler::getInstance().getScheduler().start();
 
     // Periodical statistics
     while (true)
     {
         Thread::sleep(1000);
-        Logger::getInstance().log(CpuMeter::getCpuStats());
-        CpuMeter::resetCpuStats();
-        Logger::getInstance().logStats();
+        // Logger::getInstance().log(CpuMeter::getCpuStats());
+        // CpuMeter::resetCpuStats();
+        // Logger::getInstance().logStats();
+        // MemoryProfiling::print();
     }
 }
-- 
GitLab