diff --git a/lib/utils/type_utils.h b/lib/utils/type_utils.h
new file mode 100644
index 0000000..fbe1ae9
--- /dev/null
+++ b/lib/utils/type_utils.h
@@ -0,0 +1,5 @@
+template<class T>
+T* getSingleton() {
+    static T obj;
+    return &obj;
+}
\ No newline at end of file
diff --git a/platformio.ini b/platformio.ini
index d86e4e6..36031f9 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -13,6 +13,15 @@ lib_deps =
 	arduino-libraries/Servo@^1.1.8
 	adafruit/DHT sensor library@^1.4.4
 	https://github.com/lexus2k/tinyproto#v1.0.0
+	adafruit/Adafruit Unified Sensor@^1.1.13
+	adafruit/Adafruit BMP3XX Library@^2.1.2
+	adafruit/Adafruit BNO055@^1.6.1
+
+[env:uno_mini]
+platform = atmelavr
+board = ATmega328P
+framework = arduino
+monitor_speed = 9600
 
 [env:uno]
 platform = atmelavr
diff --git a/src/AComponents/container.cpp b/src/AComponents/container.cpp
new file mode 100644
index 0000000..6029453
--- /dev/null
+++ b/src/AComponents/container.cpp
@@ -0,0 +1,40 @@
+#include "container.h"
+
+#include "Parts/AParts/interface.h"
+
+Container::Container() {
+    for (int i = 0; i < Parts + Sensors; ++i)
+        array[i] = nullptr;
+}
+
+void Container::registerPart(Interface *part) {
+    for (int i = 0; i < Parts; ++i) {
+        if (!array[i])  {
+            array[i] = part;
+            return;
+        }
+    }
+}
+
+void Container::registerSensor(Interface *sensor) {
+    for (int i = Parts; i < Parts + Sensors; ++i) {
+        if (!array[i]) {
+            array[i] = sensor;
+            return;
+        }
+    }
+}
+
+Interface *Container::find(char partID) const {
+    for (int i = 0; i < Parts + Sensors; ++i)
+        if (array[i]->getCode() == partID)
+            return array[i];
+
+    return nullptr;
+}
+
+void Container::read_data() {
+    for (int i = Parts; i < Parts + Sensors; ++i)
+        if (array[i])
+            array[i]->fun(0x00);
+}
\ No newline at end of file
diff --git a/src/AComponents/container.h b/src/AComponents/container.h
new file mode 100644
index 0000000..a4085f1
--- /dev/null
+++ b/src/AComponents/container.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#define Parts 3
+#define Sensors 2
+
+class Interface;
+
+class Container {
+public:
+  Container();
+
+  void registerPart(Interface* part);
+  void registerSensor(Interface* sensor);
+  Interface* find(char partID) const;
+  void read_data();
+
+private:
+  Interface* array[Parts + Sensors];
+};
\ No newline at end of file
diff --git a/src/AComponents/heap.h b/src/AComponents/heap.h
new file mode 100644
index 0000000..d46a6d7
--- /dev/null
+++ b/src/AComponents/heap.h
@@ -0,0 +1,123 @@
+#pragma once
+
+
+template<class T>
+class Wrapper {
+public:
+    Wrapper() : nextCall(0), ptr(nullptr) {}
+
+    Wrapper(unsigned long time, T data)
+        : nextCall(time),
+          ptr(data) {}
+
+    T get_val() const {
+        return ptr;
+    }
+
+    bool operator< (const Wrapper& obj) const {
+        return  nextCall < obj.nextCall;
+    }
+
+    T show() const {
+        return ptr;
+    }
+
+    void remove() {
+        nextCall = 0;
+        ptr = nullptr;
+    }
+
+    void setTime(unsigned long nextUpdate) {
+        nextCall = nextUpdate;
+    }
+
+    unsigned long getTime() const {
+        return nextCall;
+    }
+
+    void setPtr(T newPtr) {
+        ptr = newPtr;
+    }
+
+private:
+    unsigned long nextCall;
+    T ptr;
+};
+
+
+class Interface;
+
+template<class T>
+class Heap {
+public:
+    Heap() : size(0) {}
+
+    void insert(unsigned long nextUpdate, Interface* data) {
+        arr[size].setTime(nextUpdate);
+        arr[size].setPtr(data);
+
+        _insert((size - 1) / 2, size);
+        ++size;
+    }
+
+    void pop() {
+        if(!size) return;
+
+        --size;
+        swap(0, size);
+        arr[size].remove();
+
+        _pop(0);
+    }
+
+    T top() const {
+        return arr[0];
+    }
+
+    int get_size() const {
+        return size;
+    }
+
+    bool empty() const {
+        return !size;
+    }
+private:
+    const int capacity = 5;
+    int size;
+    T arr[5];
+
+private:
+    void _insert(int s, int target) {
+        if(!target) return;
+        if (arr[target] < arr[s]) {
+            swap(target, s);
+
+            _insert((s - 1) / 2, s);
+        }
+    }
+
+    void _pop(uint8_t parent) {
+        uint8_t smallest = parent;
+        uint8_t l = 2 * parent + 1;
+        uint8_t r = 2 * parent + 2;
+
+        if(l < size && arr[l] < arr[smallest])
+            smallest = l;
+
+
+        if(r < size && arr[r] < arr[smallest])
+            smallest = r;
+
+        if(smallest != parent) {
+            swap(parent, smallest);
+
+            _pop(smallest);
+        }
+    }
+
+    void swap(uint8_t firstIndex, uint8_t secondIndex) {
+        T t = arr[firstIndex];
+        arr[firstIndex] = arr[secondIndex];
+        arr[secondIndex] = t;
+    }
+};
\ No newline at end of file
diff --git a/src/Messages/commandmessage.cpp b/src/Messages/commandmessage.cpp
new file mode 100644
index 0000000..f4ece49
--- /dev/null
+++ b/src/Messages/commandmessage.cpp
@@ -0,0 +1,37 @@
+#include "commandmessage.h"
+#include "errors.h"
+
+CommandMessage::CommandMessage() {
+    arr[0] = 0;
+    arr[1] = 0;
+    arr[2] = 0;
+
+    arr[0] |= 1 << 7;  // Set command direction flag: 1 -> arduino to flight computer
+}
+
+
+char CommandMessage::getPart() const {
+    unsigned char mask = 63; 
+        
+    return arr[0] & mask;
+}
+    
+char CommandMessage::getCommand() const {
+    unsigned char mask = 15; 
+    return (arr[2] >> 4) & mask;
+}
+    
+int CommandMessage::getIndex() const {
+    return arr[0];
+}
+       
+void CommandMessage::sendResponse(char result) {
+    arr[0] |= 1 << 6;
+    arr[2] |= result;
+
+    HdlcSerial::sendPacket(reinterpret_cast<char*>(arr), 3); 
+}
+
+void CommandMessage::send()  {
+    HdlcSerial::sendPacket(reinterpret_cast<char*>(arr), 3); 
+}
\ No newline at end of file
diff --git a/src/Messages/commandmessage.h b/src/Messages/commandmessage.h
new file mode 100644
index 0000000..c2067e1
--- /dev/null
+++ b/src/Messages/commandmessage.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include "errors.h"
+#include "Serial/hdlc_serial.h"
+
+
+class CommandMessage{
+public:
+    CommandMessage();
+    
+    char getPart()    const;
+    char getCommand() const;
+    int  getIndex()   const;
+    void sendResponse(char result);
+    void send();
+    
+private:
+    char arr[6];
+};
\ No newline at end of file
diff --git a/src/Messages/sensormessage.h b/src/Messages/sensormessage.h
new file mode 100644
index 0000000..03ef6fa
--- /dev/null
+++ b/src/Messages/sensormessage.h
@@ -0,0 +1,64 @@
+#pragma once
+
+#include "Serial/hdlc_serial.h"
+
+template <unsigned int PayloadL>
+class SensorMessages
+{
+public:
+    SensorMessages(char partID, unsigned int type)
+        : i(0)
+    {
+        arr[0] = (partID & 127) << 1; // Cut off first bit (only 127 parts supported, as first bit is reserved for command type)
+        arr[0] |= ((PayloadL -1) & 0b00010000) >> 4; // Payload length is split between byte 0 and 1: most significant bit goes here
+        arr[1] = ((PayloadL -1) & 0b00001111) << 4; // rest here
+        arr[1] |= type & 0b00001111;
+    }
+
+    void reset(){
+        i = 0;
+    }
+
+    /// @brief Assigns the provide data to the message. WARNING: unsafe operation, be careful that the byte length match PayloadL defined beforehand
+    /// @param data Raw data pointer
+    /// @param length Length of the data to add
+    void addData(int* data)
+    {
+        memcpy(arr + 2 + i, data, sizeof(int));
+        i += sizeof(int);
+    }
+
+    /// @brief Assigns the provide data to the message. WARNING: unsafe operation, be careful that the byte length match PayloadL defined beforehand
+    /// @param data Raw data pointer
+    /// @param length Length of the data to add
+    void addData(bool* data)
+    {
+        memcpy(arr + 2 + i, data, sizeof(bool));
+        i += sizeof(bool);
+    }
+
+    /// @brief Assigns the provide data to the message. WARNING: unsafe operation, be careful that the byte length match PayloadL defined beforehand
+    /// @param data Raw data pointer
+    /// @param length Length of the data to add
+    void addData(float* data)
+    {
+        memcpy(arr + 2 + i, data, sizeof(float));
+        i += sizeof(float);
+    }
+
+    int getPart() const
+    {
+        unsigned char mask = 127;
+
+        return arr[0] & mask;
+    }
+
+    void sendMessage()
+    {
+        HdlcSerial::sendPacket(arr, PayloadL + 2); // 2 header bytes + data
+    }
+
+private:
+    unsigned char arr[PayloadL + 2];
+    char i;
+};
\ No newline at end of file
diff --git a/src/Parts/AParts/apart.h b/src/Parts/AParts/apart.h
new file mode 100644
index 0000000..54afb26
--- /dev/null
+++ b/src/Parts/AParts/apart.h
@@ -0,0 +1,71 @@
+#pragma once
+
+#include "command.h"
+#include "interface.h"
+#include "errors.h"
+
+
+
+template<class T, const int K>
+class APart  : public Interface {
+public:
+
+    virtual char getCode() const override {
+        return code;
+    };
+
+    virtual char fun(char c) override {
+        for(int i = 0; i < commandCounter; ++i) {
+            if (c == commands[i].code) {
+                if(checkState(commands[i]))
+                    return commands[i].fun(commands[i].ptr);
+                else
+                    return Errors::IncompatibleLaunchPhase;
+            }
+        }
+
+        return Errors::WrongCommandByte;
+    }
+
+    virtual bool moreUpdate() override {
+        return updateState;
+    };
+
+    virtual int getUpdatesSpeed() override {
+        return updatesSpeed;
+    }
+
+
+protected:
+    explicit APart(char c, bool updateS = false, double updatesS = 1) 
+        :   updateState(updateS),
+            updatesSpeed(updatesS * 1000),
+            code(c), 
+            commandCounter(0) {}
+
+    void addCommand(T* ptr, char code, char(*fun)(T*), int flags = LaunchPhase::Preparation | LaunchPhase::Ignition
+                                                                 | LaunchPhase::LiftOff     | LaunchPhase::Recovery ) {                                         
+        commands[commandCounter].ptr = ptr;
+        commands[commandCounter].code = code;
+        commands[commandCounter].fun = fun;
+        commands[commandCounter].flags = flags;
+        ++commandCounter;
+    }
+
+protected:
+    bool updateState;
+    int updatesSpeed;
+
+private:
+    char code;
+    int commandCounter;
+    Command<T> commands[K];
+    
+private:
+    bool checkState(const Command<T>& command) const {
+        LaunchPhase::StateEnum* launchPhase = getLaunchPhase();
+        return command.flags & (*launchPhase);
+    }
+};
+
+
diff --git a/src/Parts/AParts/asensor.h b/src/Parts/AParts/asensor.h
new file mode 100644
index 0000000..30aa890
--- /dev/null
+++ b/src/Parts/AParts/asensor.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include "apart.h"
+#include "Messages/sensormessage.h"
+#include <Arduino.h>
+
+template<class T>
+class ASensor : public APart<T, 1> {
+public:
+    explicit ASensor(char code, int updateSpeedRate = 1000) 
+        : APart<T, 1>(code),
+          updateSpeed(updateSpeedRate) {}
+
+    virtual char read_data() = 0;
+
+
+    virtual char fun(char c) override {
+        char result = Errors::Success;
+
+        if(nextReading <= millis()) {
+            result = APart<T, 1>::fun(c);
+            nextReading = millis() + updateSpeed;
+        }
+
+        return result;
+    }
+
+
+
+    void setUpdateSpeed(int s = 1) {
+        updateSpeed = s * 1000;
+    }
+
+private:
+    int updateSpeed;
+    unsigned long nextReading;
+};
diff --git a/src/Parts/AParts/command.h b/src/Parts/AParts/command.h
new file mode 100644
index 0000000..ac2415c
--- /dev/null
+++ b/src/Parts/AParts/command.h
@@ -0,0 +1,36 @@
+#pragma once
+
+struct LaunchPhase{
+
+    enum StateEnum {
+        Init                = (1 << 0),
+        Preparation         = (1 << 1),
+      //  Instalation         = (1 << 1),
+      //  FinalPreparation    = (1 << 2),
+        Ignition            = (1 << 2),
+        LiftOff             = (1 << 3),
+     // Coasting            = (1 << 5),
+     // Apogee              = (1 << 6),
+        Recovery            = (1 << 4)
+    };
+};
+
+
+inline LaunchPhase::StateEnum* getLaunchPhase()   {
+    static LaunchPhase::StateEnum launchPhase = LaunchPhase::Preparation;
+    //static LaunchPhase::StateEnum launchPhase = LaunchPhase::Preparation;
+    return &launchPhase;
+}
+
+
+template<class T>
+class Command {
+public:
+    Command() : ptr(nullptr) {}
+
+public:
+    T* ptr;
+    char code;
+    char(*fun)(T*);
+    int flags;
+};
diff --git a/src/Parts/AParts/interface.h b/src/Parts/AParts/interface.h
new file mode 100644
index 0000000..ebe58d4
--- /dev/null
+++ b/src/Parts/AParts/interface.h
@@ -0,0 +1,14 @@
+#pragma once
+
+class State;
+
+class Interface {
+public:
+    virtual char fun(char) = 0;
+    virtual ~Interface() = default;
+
+    virtual char getCode() const = 0;
+    virtual void update() {}
+    virtual bool moreUpdate() = 0;
+    virtual int  getUpdatesSpeed() = 0;
+};
\ No newline at end of file
diff --git a/src/Parts/Parts/igniter.cpp b/src/Parts/Parts/igniter.cpp
new file mode 100644
index 0000000..af2187d
--- /dev/null
+++ b/src/Parts/Parts/igniter.cpp
@@ -0,0 +1,28 @@
+#include "igniter.h"
+#include <Arduino.h>
+#include "myarduino.h"
+
+Igniter::Igniter() : APart(0x02, true, 0.1) { 
+        pinMode(IgniterPin, OUTPUT);
+        digitalWrite(IgniterPin, LOW);
+
+        addCommand(this,  0x00, [](Igniter* t) { return t->ignite(); });
+}
+
+void Igniter::update() {
+
+    // Turn off igniter pin after ignitionOnMillis
+    if(millis() > (lastIgniteMillis + ignitionOnMillis)){
+        lastIgniteMillis = 0;
+        digitalWrite(IgniterPin, LOW); 
+    }
+
+}
+
+char Igniter::ignite() {
+    digitalWrite(IgniterPin, HIGH);  
+    lastIgniteMillis = millis();
+
+    return Errors::Success;
+}
+
diff --git a/src/Parts/Parts/igniter.h b/src/Parts/Parts/igniter.h
new file mode 100644
index 0000000..b42df6f
--- /dev/null
+++ b/src/Parts/Parts/igniter.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "Parts/AParts/apart.h"
+
+class Igniter : public APart<Igniter, 1> {
+public:
+    Igniter();
+    void update();
+
+private:
+    static const int IgniterPin = 2;
+    static const unsigned int ignitionOnMillis = 1000;
+
+private:
+    unsigned long lastIgniteMillis = 0;
+    char ignite();
+};
\ No newline at end of file
diff --git a/src/Parts/Parts/marduino.cpp b/src/Parts/Parts/marduino.cpp
new file mode 100644
index 0000000..b917307
--- /dev/null
+++ b/src/Parts/Parts/marduino.cpp
@@ -0,0 +1,40 @@
+#include "marduino.h"
+#include "Serial/hdlc_serial.h"
+#include "Messages/sensormessage.h"
+#include "errors.h"
+#include "Messages/commandmessage.h"
+#include <Arduino.h>
+#include "myservo.h"
+#include "type_utils.h"
+
+MArduno::MArduno() : APart(0x00, true) {
+
+
+    addCommand(this,  0x00, [](MArduno* t) { return t->resetArduino(); });
+    addCommand(this,  0x01, [](MArduno* t) { return t->keepAlive(); });
+}
+
+char MArduno::resetArduino() {
+    resetFunc();
+    return Errors::Success;
+}
+
+char MArduno::keepAlive(){
+    this->lastKeepAliveMillis = millis();
+    this->timeoutTriggered = false;
+    return Errors::Success;
+}
+
+void MArduno::update(){
+
+    if(!timeoutTriggered && millis() > (lastKeepAliveMillis + timeout)){
+        timeoutTriggered = true;
+        onTimeout();
+    }
+}
+
+//Trigger parachute in case of timeout
+void MArduno::onTimeout(){
+    MyServo* servo = getSingleton<MyServo>();
+    servo->open();
+}
\ No newline at end of file
diff --git a/src/Parts/Parts/marduino.h b/src/Parts/Parts/marduino.h
new file mode 100644
index 0000000..811ce02
--- /dev/null
+++ b/src/Parts/Parts/marduino.h
@@ -0,0 +1,22 @@
+#include "Parts/AParts/apart.h"
+
+
+class  MArduno : public APart<MArduno, 5> {
+    enum States {};
+public :
+    MArduno();
+    void update();
+
+private:
+    void(* resetFunc) (void) = 0;
+
+
+    static const int timeout = 10000;
+    unsigned long lastKeepAliveMillis = 0;
+    bool timeoutTriggered = false;
+
+    char resetArduino();
+    char keepAlive();
+
+    void onTimeout();
+};
\ No newline at end of file
diff --git a/src/Parts/Parts/myservo.cpp b/src/Parts/Parts/myservo.cpp
new file mode 100644
index 0000000..6d0371c
--- /dev/null
+++ b/src/Parts/Parts/myservo.cpp
@@ -0,0 +1,29 @@
+#include "myservo.h"
+#include <Arduino.h>
+
+MyServo::MyServo()
+    : APart(0x01),
+      myservo(),
+      closedPos(90),
+      openPos(-10) {
+
+        myservo.attach(ServoPin);  
+        myservo.write(closedPos);
+
+        addCommand(this,  0x00, [](MyServo* t) { return t->close(); });
+        addCommand(this,  0x01, [](MyServo* t) { return t->open();  });
+}
+
+char MyServo::close() {
+    myservo.write(closedPos);
+    return Errors::Success;
+}
+
+char MyServo::open() {
+    myservo.write(openPos);
+    return Errors::Success;
+}
+
+
+
+
diff --git a/src/Parts/Parts/myservo.h b/src/Parts/Parts/myservo.h
new file mode 100644
index 0000000..7653d88
--- /dev/null
+++ b/src/Parts/Parts/myservo.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include <Servo.h>
+#include "Parts/AParts/apart.h"
+
+class MyServo : public APart<MyServo, 2> {
+public:
+  MyServo();
+  
+private:
+  Servo      myservo; 
+  int        closedPos; 
+  int        openPos;
+
+  static const int ServoPin = 4;
+
+public:
+  char close();
+  char open();
+};
diff --git a/src/Parts/Parts/templatePart.cpp b/src/Parts/Parts/templatePart.cpp
new file mode 100644
index 0000000..cc3871c
--- /dev/null
+++ b/src/Parts/Parts/templatePart.cpp
@@ -0,0 +1,15 @@
+#include "templatePart.h"
+
+PartName::PartName()
+    : APart(0x01) {
+        
+    addCommand(this,  0x00, [](PartName* t) { return t->commandFunction(); }, LaunchPhase::Preparation | LaunchPhase::LiftOff | LaunchPhase::Recovery);
+}
+
+char PartName::commandFunction() {
+    return Errors::Success;
+}
+
+
+
+
diff --git a/src/Parts/Parts/templatePart.h b/src/Parts/Parts/templatePart.h
new file mode 100644
index 0000000..2bf359c
--- /dev/null
+++ b/src/Parts/Parts/templatePart.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include "Parts/AParts/apart.h"
+
+class PartName : public APart<PartName, 1> {
+public:
+  PartName();
+  
+private:
+  char commandFunction();
+};
diff --git a/src/Parts/Sensors/orientationsensor.cpp b/src/Parts/Sensors/orientationsensor.cpp
new file mode 100644
index 0000000..f136e47
--- /dev/null
+++ b/src/Parts/Sensors/orientationsensor.cpp
@@ -0,0 +1,56 @@
+#include "orientationsensor.h"
+
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <utility/imumaths.h>
+
+#define QUAT_LENGTH = sizeof(double)*4;
+
+OrientationSensor::OrientationSensor()
+    : ASensor(10, 91),
+      
+      bno(Adafruit_BNO055(55))
+{
+
+    sm = new SensorMessages<sizeof(int)*4 + sizeof(bool)>(10, 1);
+
+    addCommand(this, 0x00, [](OrientationSensor *t)
+               { return t->read_data(); });
+
+    initializeSuccess = bno.begin(adafruit_bno055_opmode_t::OPERATION_MODE_IMUPLUS);
+
+    bno.setExtCrystalUse(true);
+
+}
+
+void OrientationSensor::update()
+{
+    this->read_data();
+}
+
+char OrientationSensor::read_data()
+{
+
+    if(!initializeSuccess)
+        return Errors::JustWrong;
+
+    imu::Quaternion quat = bno.getQuat();
+    bool calibrationState = bno.isFullyCalibrated();
+
+    int w = quat.w() * 32767;
+    int x = quat.x() * 32767;
+    int y = quat.y() * 32767;
+    int z = quat.z() * 32767;
+
+    sm->reset();
+    
+    sm->addData(&w);
+    sm->addData(&x);
+    sm->addData(&y);
+    sm->addData(&z);
+    sm->addData(&calibrationState);
+
+    sm->sendMessage();
+
+    return 0;
+}
\ No newline at end of file
diff --git a/src/Parts/Sensors/orientationsensor.h b/src/Parts/Sensors/orientationsensor.h
new file mode 100644
index 0000000..24280aa
--- /dev/null
+++ b/src/Parts/Sensors/orientationsensor.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "Parts/AParts/asensor.h"
+#include <Adafruit_BNO055.h>
+
+class OrientationSensor : public ASensor<OrientationSensor> {
+public:
+    OrientationSensor();
+
+    virtual void update()    override;
+    virtual char read_data() override;
+
+private:
+    Adafruit_BNO055 bno;
+    bool initializeSuccess;
+    SensorMessages<sizeof(int)*4 + sizeof(bool)>* sm;
+
+};
\ No newline at end of file
diff --git a/src/Parts/Sensors/photoresistorsensor.cpp b/src/Parts/Sensors/photoresistorsensor.cpp
new file mode 100644
index 0000000..cbe20a6
--- /dev/null
+++ b/src/Parts/Sensors/photoresistorsensor.cpp
@@ -0,0 +1,23 @@
+#include "photoresistorsensor.h"
+#include <Arduino.h>
+
+PhotoResistorSensor::PhotoResistorSensor() : ASensor(0x52, 2) {
+    pinMode(A2, INPUT);
+
+    addCommand(this,  0x00, [](PhotoResistorSensor* t) { return t->read_data(); });
+}
+
+char PhotoResistorSensor::read_data() {
+    //float data = analogRead(2); 
+	//Serial.println(value, DEC); // light intensity
+
+   // return send_Message(data);
+
+   return Errors::Success;
+}
+
+void PhotoResistorSensor::update()   {
+
+}
+
+
diff --git a/src/Parts/Sensors/photoresistorsensor.h b/src/Parts/Sensors/photoresistorsensor.h
new file mode 100644
index 0000000..bbcd52a
--- /dev/null
+++ b/src/Parts/Sensors/photoresistorsensor.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include "Parts/AParts/asensor.h"
+
+class PhotoResistorSensor : public ASensor<PhotoResistorSensor> {
+public:
+    PhotoResistorSensor();
+
+    virtual void update()    override;
+    virtual char read_data() override;
+};
\ No newline at end of file
diff --git a/src/Parts/Sensors/pressuresensor.cpp b/src/Parts/Sensors/pressuresensor.cpp
new file mode 100644
index 0000000..fbf52e2
--- /dev/null
+++ b/src/Parts/Sensors/pressuresensor.cpp
@@ -0,0 +1,46 @@
+#include "pressuresensor.h"
+#include <Wire.h>
+#include <SPI.h>
+#include <Adafruit_Sensor.h>
+#include "Adafruit_BMP3XX.h"
+
+
+PressureSensor::PressureSensor() 
+    : ASensor(11, 111),
+      bmp() {
+
+    sm = new SensorMessages<sizeof(float)*2>(11, 1);
+
+    addCommand(this,  0x00, [](PressureSensor* t) { return t->read_data(); });
+
+    initializeSuccess = bmp.begin_I2C();   // hardware I2C mode, can pass in address & alt Wire
+    
+    // Set up oversampling and filter initialization
+    bmp.setTemperatureOversampling(BMP3_OVERSAMPLING_8X);
+    bmp.setPressureOversampling(BMP3_OVERSAMPLING_4X);
+    bmp.setIIRFilterCoeff(BMP3_IIR_FILTER_COEFF_3);
+    bmp.setOutputDataRate(BMP3_ODR_200_HZ);
+
+}
+
+void PressureSensor::update() {
+    this->read_data();
+}
+
+char PressureSensor::read_data() {
+
+    // if(!initializeSuccess)
+    //     return Errors::JustWrong;
+
+    float pressure =  bmp.readPressure();
+    float temperature = bmp.readTemperature();
+
+    sm->reset();
+    
+    sm->addData(&pressure);
+    sm->addData(&temperature);
+
+    sm->sendMessage();
+    
+    return Errors::Success;
+}
\ No newline at end of file
diff --git a/src/Parts/Sensors/pressuresensor.h b/src/Parts/Sensors/pressuresensor.h
new file mode 100644
index 0000000..0b906b5
--- /dev/null
+++ b/src/Parts/Sensors/pressuresensor.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "Parts/AParts/asensor.h"
+#include "Adafruit_BMP3XX.h"
+
+class PressureSensor : public ASensor<PressureSensor> {
+public:
+    PressureSensor();
+
+    virtual void update()    override;
+    virtual char read_data() override;
+
+private:
+    Adafruit_BMP3XX bmp;
+    bool initializeSuccess;
+    SensorMessages<sizeof(float)*2>* sm;
+};
\ No newline at end of file
diff --git a/src/Parts/Sensors/templateSensor.cpp b/src/Parts/Sensors/templateSensor.cpp
new file mode 100644
index 0000000..5a7a645
--- /dev/null
+++ b/src/Parts/Sensors/templateSensor.cpp
@@ -0,0 +1,15 @@
+#include "templateSensor.h"
+
+SensorName::SensorName() : ASensor(0x90) {
+
+    addCommand(this,  0x00, [](SensorName* t) { return t->read_data(); });
+}
+
+
+void SensorName::update() {
+    
+}
+
+char SensorName::read_data() {
+    return Errors::Success;
+}
\ No newline at end of file
diff --git a/src/Parts/Sensors/templateSensor.h b/src/Parts/Sensors/templateSensor.h
new file mode 100644
index 0000000..095a2e1
--- /dev/null
+++ b/src/Parts/Sensors/templateSensor.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "Parts/AParts/asensor.h"
+
+class SensorName : public ASensor<SensorName> {
+public:
+    SensorName();
+
+    virtual void update()    override;
+    virtual char read_data() override;
+
+private:
+
+};
\ No newline at end of file
diff --git a/src/Serial/hdlc_serial.cpp b/src/Serial/hdlc_serial.cpp
new file mode 100644
index 0000000..4e30ddf
--- /dev/null
+++ b/src/Serial/hdlc_serial.cpp
@@ -0,0 +1,46 @@
+#include "hdlc_serial.h"
+#include "myarduino.h"
+#include "Parts/AParts/apart.h"
+#include "Messages/commandmessage.h"
+#include "Messages/sensormessage.h"
+#include "errors.h"
+#include "type_utils.h"
+
+
+namespace HdlcSerial {
+    
+    MyArduino* myArduino = getSingleton<MyArduino>();
+
+    char buf[128];
+    tinyproto::Light proto;
+    tinyproto::Hdlc hdlc(static_cast<void*>(buf), 128); 
+
+
+    void onReceive(tinyproto::IPacket &pkt) {
+        void* data = pkt.data();
+        CommandMessage* msg1 = static_cast<CommandMessage*>(data);
+
+        char result = myArduino->find(msg1->getPart(), msg1->getCommand());
+        msg1->sendResponse(result);
+    }
+
+    void startSerial() {
+        hdlc.setReceiveCallback(onReceive);
+        hdlc.enableCheckSum();
+        hdlc.begin();
+
+        proto.enableCheckSum();
+        proto.beginToSerial();
+    }
+
+    void receive() {
+        if (Serial.available()) {
+            uint8_t byte = Serial.read();
+            hdlc.run_rx( &byte, 1 ); 
+        }
+    }
+
+    void sendPacket(char* data_buffer, int numberBytes){
+        proto.write(data_buffer, numberBytes);
+    }
+}
\ No newline at end of file
diff --git a/src/Serial/hdlc_serial.h b/src/Serial/hdlc_serial.h
new file mode 100644
index 0000000..c5f0d97
--- /dev/null
+++ b/src/Serial/hdlc_serial.h
@@ -0,0 +1,10 @@
+#pragma once
+#include <TinyProtocol.h>
+
+namespace HdlcSerial {
+    void startSerial();
+
+    void receive();
+
+    void sendPacket(char* data_buffer, int n = 7);
+}
\ No newline at end of file
diff --git a/src/errors.h b/src/errors.h
new file mode 100644
index 0000000..2647130
--- /dev/null
+++ b/src/errors.h
@@ -0,0 +1,9 @@
+#pragma once
+
+enum Errors {
+    Success                  = 0x00,
+    IncompatibleLaunchPhase  = 0x01,
+    WrongPartByte            = 0x02,
+    WrongCommandByte         = 0x03,
+    JustWrong                = 0x04
+};
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index f390014..e86b83f 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,87 +1,35 @@
 #include <Arduino.h>
 
-#include "temph.h"
-#include "mysoftwareserial.h"
-#include "tempanalog.h"
-#include "myservo.h"
+#include "Serial/hdlc_serial.h"
+#include "myarduino.h"
 
-#include <serial.hpp>
+#include "type_utils.h"
 
-MyServo myServo;
-AnalogThemp analogTempSensor;
-HumTemp humTempSensor;
-AbstractSensor* arr[2];
+#include "Parts/Parts/myservo.h"
+#include "Parts/Parts/igniter.h"
+#include "Parts/Parts/marduino.h"
+#include "Parts/Sensors/photoresistorsensor.h"
+#include "Parts/Sensors/pressuresensor.h"
+#include "Parts/Sensors/orientationsensor.h"
 
+MyArduino* myArduino = getSingleton<MyArduino>();
 
-
-
-// void setup() {
-//   Serial.begin(9600);
-//   myServo.init();
-
-//   arr[0] = &analogTempSensor;
-//   arr[1] = &humTempSensor;
-
-//   analogTempSensor.init();
-//   humTempSensor.init();
-
-//   analogTempSensor.listen();
-// }
-
-// char message;
-
-// void input() {
-//   while(Serial.available()) {
-//       char c = Serial.read();
-
-//         if (c == 48 || c == 49)
-//           message = c;
-//         else if (c == 13) {
-//           if (message == 48) {
-//             myServo.open();
-//             Serial.write("OPEN!!!\n");
-//           }
-//           else if (message == 49){
-//             myServo.close();
-//             Serial.write("CLOSE!!!\n");
-//           }
-//           message = 50;
-//         }
-//   }
-// }
-
-// void loop() {
-//   for  (auto sensor : arr) {
-//     sensor->listen();
-//     sensor->loop();
-//     input();
-//     delay(1000);
-//   }
-// }
-
-void onPacket(RSSPacket &packet){
-
-}
-
-void setup(){
-
+void setup() {
   Serial.begin(9600);
 
+  myArduino->registerPart(getSingleton<MArduno>());
+  myArduino->registerPart(getSingleton<MyServo>());
+  myArduino->registerPart(getSingleton<Igniter>());
 
-  startHardwareSerial(onPacket);
+  myArduino->registerSensor(getSingleton<PressureSensor>());
+  myArduino->registerSensor(getSingleton<OrientationSensor>());
 
+  HdlcSerial::startSerial();
 }
 
-void loop(){
-
-  runRead();
-
-  RSSPacket packet;
-
-  packet.command = 0x22;
-  packet.payloadLength = 0;
-
-  delay(1000);
-
-  write(packet);
+void loop() { 
+  HdlcSerial::receive(); 
+  myArduino->update(); 
+  myArduino->read_data();
 }
+
diff --git a/src/myarduino.cpp b/src/myarduino.cpp
new file mode 100644
index 0000000..c543074
--- /dev/null
+++ b/src/myarduino.cpp
@@ -0,0 +1,50 @@
+#include "myarduino.h"
+#include <Arduino.h>
+#include "Parts/AParts/interface.h"
+#include "errors.h"
+
+void MyArduino::registerPart(Interface* part) {
+    array.registerPart(part);
+
+    if(part->moreUpdate())
+        addUpdateComponent(part->getUpdatesSpeed() + millis(), part);
+}
+
+void MyArduino::registerSensor(Interface* sensor){
+    array.registerSensor(sensor);
+}
+
+void MyArduino::update() {
+    if(!heap.empty()) {
+        if(heap.top().getTime() <= millis()) {
+            Interface* obj = heap.top().get_val();
+            heap.pop();
+
+            if(obj->moreUpdate())  {
+                obj->update();
+
+                if(obj->moreUpdate()) 
+                    addUpdateComponent(obj->getUpdatesSpeed() + millis(), obj);
+            }
+        }
+    }
+}
+
+
+char MyArduino::find(unsigned char part, char code) {
+    Interface* ptr = array.find(part);
+
+    if(!ptr)
+        return Errors::WrongPartByte;
+
+    return ptr->fun(code);
+}
+
+void MyArduino::read_data() {
+    array.read_data();
+}
+
+void MyArduino::addUpdateComponent(unsigned long nextUpdate, Interface* component) {
+    heap.insert(nextUpdate, component);
+}
+
diff --git a/src/myarduino.h b/src/myarduino.h
new file mode 100644
index 0000000..36ff6c5
--- /dev/null
+++ b/src/myarduino.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#define PartsCount 3
+#define SensorsCount 2
+
+#include <Arduino.h>
+#include "AComponents/heap.h"
+#include "AComponents/container.h"
+
+class Interface;
+
+class MyArduino {
+public:
+  void update();
+  void read_data();
+
+  char find(unsigned  char part, char code);
+
+  void registerPart(Interface*r);
+  void registerSensor(Interface*r);
+  void addUpdateComponent(unsigned long nextUpdate, Interface* component);
+
+private:  
+  Heap<Wrapper<Interface*>> heap;
+  Container array;
+};
\ No newline at end of file
diff --git a/src/myservo.cpp b/src/myservo.cpp
deleted file mode 100644
index 8c226e8..0000000
--- a/src/myservo.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#include "myservo.h"
-#include <Arduino.h>
-
-
-#define ServoPin 8
-
-
-MyServo::MyServo()
-    : myservo(),
-      closedPos(20),
-      openPos(120),
-      servoState(Closed) { }
-
-void MyServo::init() {
-   myservo.attach(ServoPin);  // attaches the servo on pin 9 to the servo object
-   myservo.write(closedPos);
-}
-
-void MyServo::message() const  {   
-} 
-
-
-void MyServo::close() {
-    if (servoState == Open) {
-                
-
-        for (int i = openPos; closedPos <= i; --i) {
-            myservo.write(i);
-            delay(15);
-        }
-           
-        //myservo.write(closedPos);
-
-        servoState = Closed;
-    }
-}
-
-void MyServo::open() {
-    if (servoState == Closed) {
-        /*
-        for (int i = closedPos; i <= openPos; ++i) {
-            myservo.write(i);
-            delay(15);
-        }
-        */
-                   myservo.write(openPos);
-
-        servoState = Open;
-    }
-}
-
-
-
-
diff --git a/src/myservo.h b/src/myservo.h
deleted file mode 100644
index b96e2b3..0000000
--- a/src/myservo.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#pragma once
-
-#include <Servo.h>
-
-class MyServo {
-    enum ServoState { Closed = 0, Open };
-
-public:
-
-  MyServo();
-
-  void init();
-
-  void message() const;
-
-  void close();
-
-  void open();
-
-
-private:
-    Servo      myservo; 
-    int        closedPos; 
-    int        openPos;
-    ServoState servoState;
-};
diff --git a/src/mysoftwareserial.h b/src/mysoftwareserial.h
deleted file mode 100644
index c2d5528..0000000
--- a/src/mysoftwareserial.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#pragma once
-
-#include <SoftwareSerial.h>
-
-class AbstractSensor: public SoftwareSerial {
-
-public:
-  AbstractSensor(int rxPins, int txPins) 
-  : SoftwareSerial(rxPins, txPins) {}
-
-  virtual void message() const;
-  virtual void loop()  = 0;
-};
\ No newline at end of file
diff --git a/src/serial.cpp b/src/serial.cpp
deleted file mode 100644
index 163453a..0000000
--- a/src/serial.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-#include <serial.hpp>
-#include <TinyProtocol.h>
-
-
-u8* buffer[128];
-
-tinyproto::Hdlc hdlc(buffer, 128);
-
-tinyproto::StaticPacket<64> tinyPacket;
-
-RSSPacket rssPacket;
-
-u8 message_index;
-
-void (*on_receive)(RSSPacket &pkt) = nullptr;
-
-int write(void *pdata, const void *buffer, int size){
-    return Serial.write((char*)buffer, size);
-}
-
-int read(void *pdata, void *buffer, int size){
-    return Serial.readBytes((char*)buffer, size);
-}
-
-void onReceive(tinyproto::IPacket &pkt){
-
-    rssPacket.index = pkt.getByte();
-    rssPacket.command = pkt.getByte();
-    rssPacket.payloadLength = pkt.getByte();
-
-    for(u8 i = 0; i < rssPacket.payloadLength; i++)
-        tinyPacket.put(rssPacket.payload[i]);
-
-    on_receive(rssPacket);
-}
-
-void startHardwareSerial(void (*on_receive_callback)(RSSPacket &pkt)){
-
-    on_receive = on_receive_callback;
-
-    Serial.begin(9600);
-
-    hdlc.enableCheckSum();
-
-    hdlc.setReceiveCallback(onReceive);
-
-    hdlc.begin();
-    
-}
-
-
-char* read_write_buffer[64];
-
-void runRead(){
-
-    
-    if(Serial.available() < 1){
-        return;
-    }
-
-    size_t readBytes = Serial.readBytes((char*)read_write_buffer, 64);
-
-
-    hdlc.run_rx(read_write_buffer, readBytes);
-}
-
-void write(RSSPacket &packet){
-
-    packet.index = message_index++;
-
-    tinyPacket.clear();
-    tinyPacket.put(packet.index);
-    tinyPacket.put(packet.command);
-    tinyPacket.put(packet.payloadLength);
-
-    for(u8 i = 0; i < packet.payloadLength; i++)
-        tinyPacket.put(packet.payload[i]);
-
-    hdlc.write(tinyPacket);
-
-    size_t toSend = hdlc.run_tx(read_write_buffer, 64);
-
-    Serial.write((char*) read_write_buffer, toSend);
-}
-
diff --git a/src/serial.hpp b/src/serial.hpp
deleted file mode 100644
index d5a4619..0000000
--- a/src/serial.hpp
+++ /dev/null
@@ -1,42 +0,0 @@
-#pragma once
-
-#include <Arduino.h>
-
-
-// namespace RSS {
-//     namespace HdlcSerial {
-
-        struct RSSPacket {
-
-            /**
-             * The index of the message. (Rolls over)
-            */
-            u8 index;
-
-            /**
-             * The command to execute
-            */
-            u8 command;
-
-            /**
-             * Actual length of the payload
-            */
-            size_t payloadLength;
-
-            /**
-             * Payload of the packet (any additional data)
-            */
-            u8 payload[48];
-            
-        };
-        
-
-        void startHardwareSerial(void (*on_receive_callback)(RSSPacket &pkt));
-
-        void runRead();
-
-        void write(RSSPacket& packet);
-
-//     }
-// }
-
diff --git a/src/tempanalog.cpp b/src/tempanalog.cpp
deleted file mode 100644
index a153627..0000000
--- a/src/tempanalog.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-#include "tempanalog.h"
-
-#include <Arduino.h>
-
-
-int ThermistorPin = A0;
-int Vo;
-float R1 = 10000; // value of R1 on board
-float logR2, R2, T;
-float c1 = 0.001129148, c2 = 0.000234125, c3 = 0.0000000876741;
-
-AnalogThemp::AnalogThemp()
-    : AbstractSensor(ThermistorPin, 1) { }
-
-void AnalogThemp::init() {}
-
-void AnalogThemp::message() const  {
-    Serial.write("Temperature analog sensor activated! \n");
-} 
-
-void AnalogThemp::loop()   {
-  Vo = analogRead(ThermistorPin);
-  R2 = R1 * (1023.0 / (float)Vo - 1.0); //calculate resistance on thermistor
-  logR2 = log(R2);
-  T = (1.0 / (c1 + c2*logR2 + c3*logR2*logR2*logR2)); // temperature in Kelvin
-  T = T - 273.15; //convert Kelvin to Celcius
- // T = (T * 9.0)/ 5.0 + 32.0; //convert Celcius to Farenheit
-  Serial.print("Temperature: "); 
-  Serial.print(T);
-  Serial.println(" C"); 
-
-  delay(500);
-}
-
diff --git a/src/tempanalog.h b/src/tempanalog.h
deleted file mode 100644
index 50dffae..0000000
--- a/src/tempanalog.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#pragma once
-
-#include "mysoftwareserial.h"
-
-class AnalogThemp : public AbstractSensor {
-
-public:
-
-  AnalogThemp();
-
-  void init();
-
-  virtual void message() const override;
-
-  virtual void loop()  override;
-
-};
diff --git a/src/temph.cpp b/src/temph.cpp
deleted file mode 100644
index 99aa81e..0000000
--- a/src/temph.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-#include "temph.h"
-
-#include <Adafruit_Sensor.h>
-
-#define DHTPIN 2             // Digital pin connected to the DHT sensor 
-#define DHTTYPE    DHT11     // DHT 11
-
-DHT_Unified dht(DHTPIN, DHTTYPE);
-
-uint32_t delayMS;
-
-HumTemp::HumTemp() 
-    :  AbstractSensor(2, 1) {}
-
-
-void HumTemp::init() {
-      dht.begin();
-      
-      // Print temperature sensor details.
-      sensor_t sensor;
-      dht.temperature().getSensor(&sensor);
-      dht.humidity().getSensor(&sensor);
-     
-      // Set delay between sensor readings based on sensor details.
-      delayMS = sensor.min_delay / 10000;
-  
-
-}
-
-void HumTemp::message() const  {
-    Serial.write("Temperature and Humidity sensort activated! \n");
-} 
-
-void HumTemp::loop()   {
-      // Delay between measurements.
-      delay(delayMS);
-
-      // Get temperature event and print its value.
-      sensors_event_t event;
-      dht.temperature().getEvent(&event);
-      if (isnan(event.temperature)) {
-        Serial.println(F("Error reading temperature!"));
-      }
-      else {
-        Serial.print(F("Temperature: "));
-        Serial.print(event.temperature);
-        Serial.println(F("°C"));
-      }
-
-      // Get humidity event and print its value.
-      dht.humidity().getEvent(&event);
-      if (isnan(event.relative_humidity)) {
-        Serial.println(F("Error reading humidity!"));
-      }
-      else {
-        Serial.print(F("Humidity: "));
-        Serial.print(event.relative_humidity);
-        Serial.println(F("%"));
-      }
-}
-
diff --git a/src/temph.h b/src/temph.h
deleted file mode 100644
index 137d883..0000000
--- a/src/temph.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#pragma once
-
-#include "mysoftwareserial.h"
-#include <DHT.h>
-#include <DHT_U.h>
-
-
-
-class HumTemp : public AbstractSensor {
-
-public:
-
-  HumTemp();
-
-  void init();
-
-  virtual void message() const override;
-
-  virtual void loop()  override;
-
-};
diff --git a/test/test_serial.cpp b/test/test_serial.cpp
new file mode 100644
index 0000000..c5fe760
--- /dev/null
+++ b/test/test_serial.cpp
@@ -0,0 +1,14 @@
+#include <unity.h>
+
+void setUp(void) {
+    // set stuff up here
+}
+
+void tearDown(void) {
+    // clean stuff up here
+}
+
+int main( int argc, char **argv) {
+    UNITY_BEGIN();
+    UNITY_END();
+}