diff --git a/Source/Control/Autopilot.cpp b/Source/Control/Autopilot.cpp
new file mode 100644
index 0000000..5b8a172
--- /dev/null
+++ b/Source/Control/Autopilot.cpp
@@ -0,0 +1,3 @@
+#include "Autopilot.h"
+
+DroneStatus MainDrone;
\ No newline at end of file
diff --git a/Source/Control/Autopilot.h b/Source/Control/Autopilot.h
new file mode 100644
index 0000000..d2615be
--- /dev/null
+++ b/Source/Control/Autopilot.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#ifndef AUTOPILOT_H
+#define AUTOPILOT_H
+
+#include "Protocol.h"
+
+struct DroneStatus
+{
+  bool MainActive = false; // Индикатор снятия с охраны
+  bool AutoPilotActive = false; // Индикатор актив. автопилота
+  bool ManualInputAllowed = false; // Индикатор возможности ручного управления (безопасного)
+  bool AltChangeAllowed = false; // Индикатор возможности смены целевой высоты в режиме удержания позиции
+  bool PointBeginNeed = true; // Индикатор необходимости указания начальной точки
+  bool MissionExecute = false; // Инидикатор выполняется ли миссия
+  
+  bool MotorMoutionTestEnable = false;
+  unsigned char MotorTestId = 0;
+  unsigned long TimeMotorTestEnable = 0;
+  
+  bool NeedSendRawCalibData = false;
+  SENSOR_ID Sensor = SENSOR_ID::ACC;
+  unsigned long TimeLastSend = 0;
+  unsigned long TimeStartSend = 0;
+  bool NeedSaveNewEEPROMData = false;
+  bool EEPROMSaved = false;
+  bool NeedSaveNewFLASHData = false;
+  bool FLASHSaved = false;
+  
+  bool EEPROMInit = false;
+  bool BarInit = false;
+  bool IMUInit = false;
+  bool GPSInit = false;
+  bool TOFInit = false;
+  bool OFInit = false;
+  bool ExternMagInit = false;
+  
+  float AvgEngineThrust = 0.0f;
+  float MaxHeightGroundMode = 5.0f;
+  
+  ENGINE_STATUS engine = ENGINE_STATUS::disable;
+  SYS_MODE Mode = SYS_MODE::manual;
+  STATUS_MODE StatusMode = STATUS_MODE::on_ground;
+  NAV_SYS_MODE Nav = NAV_SYS_MODE::inertial;
+};
+
+extern DroneStatus MainDrone;
+
+#endif
\ No newline at end of file
diff --git a/Source/Control/Protocol.cpp b/Source/Control/Protocol.cpp
new file mode 100644
index 0000000..df333a9
--- /dev/null
+++ b/Source/Control/Protocol.cpp
@@ -0,0 +1,2 @@
+#include "Protocol.h"
+
diff --git a/Source/Control/Protocol.h b/Source/Control/Protocol.h
new file mode 100644
index 0000000..3626f09
--- /dev/null
+++ b/Source/Control/Protocol.h
@@ -0,0 +1,296 @@
+#pragma once
+
+#ifndef PROTOCOL_H
+#define PROTOCOL_H
+
+#define MAX_LEN_MISSION_NAME 64
+#define MAX_COUNT_MISSION_POINT 100
+
+enum class MESSAGES_ID : unsigned char
+{
+  SysInfo = 1,
+  GyroInfo = 2,
+  AccelInfo = 3,
+  GpsInfo = 4,
+  InertialInfo = 5,
+  BatteryInfo = 6,
+  Ack = 7,
+  StatusCommand = 8,
+  StatusAllCommands = 9,
+  Command = 10,
+  RequestReg = 11,
+  ResponseReg = 12,
+  Auth = 13,
+  OpenKey = 14,
+  MissionCount = 15,
+  MissionItem = 16,
+  MissionItemAck = 17,
+  MissionRequestItem = 18,
+  RequestLastMessage = 19,
+  ConnectionTest = 20,
+  MissionProgress = 21,
+  CountMenuCategories = 32,
+  CategoriesMenu = 33,
+  CategoriesParameters = 34,
+  RawCalibData = 35
+};
+enum class COMMANDS_ID : unsigned char
+{
+  ChangeNav = 1,
+  ChangeSpeed = 2,
+  Land = 3,
+  GoHome = 4,
+  StopEngine = 5,
+  StartEngine = 6,
+  Pause = 7,
+  Continue = 8,
+  GoToGlobal = 9,
+  GoToLocal = 10,
+  MissionStart = 11,
+  SetParameter = 15,
+  ProcessingCalibration = 16,
+  SetCalibrationData = 17,
+  MotorMoutionTest = 18
+};
+enum class ERROR_CODE_COMMAND : unsigned char
+{
+  No_error = 0,
+  Speed_has_already_been_set = 1,
+  Engines_are_already_running = 2,
+  Engines_have_already_been_stopped = 3,
+  UAV_is_already_on_the_ground = 4,
+  UAV_is_already_landing = 5,
+  UAV_is_already_moving = 6,
+  Mission_has_not_been_launched = 7,
+  Mission_is_already_on_pause = 8,
+  Mission_was_not_on_pause = 9,
+  Mission_was_not_launched = 10,
+  This_NavSys_has_already_been_applied = 11,
+  Lost_connection_with_controller = 12,
+  The_command_is_canceled_when_a_new_one_is_received = 13,
+  Error_reading_the_mission_file = 14,
+  Error_validating_the_mission_file = 15,
+  This_attack_event_already_exists = 16,
+  Error_creating_the_configuration_file_moa = 17,
+  Error_saving_the_configuration_file_moa = 18,
+  Error_deleting_the_attack_event = 19,
+  This_attack_event_cannot_be_deleted = 20,
+  This_attack_event_does_not_exist = 21,
+  Gps_not_init = 22,
+};
+enum class SENSOR_ID : unsigned char
+{
+  ACC = 0,
+  IMU_MAG = 1,
+  EXTERNAL_MAG = 2,
+  LEVEL_HOR = 3
+};
+enum class ENGINE_STATUS : unsigned char
+{
+  disable = 0,
+  enable = 1
+};
+enum class STATUS_MODE : unsigned char
+{
+  on_ground = 1,
+  taking_off = 2,
+  fly = 3,
+  landing = 4
+};
+enum class SYS_MODE : unsigned char
+{
+  take_off = 1,
+  land = 2,
+  go_home = 3,
+  auto_mode = 4,
+  hold = 5,
+  manual = 6
+};
+enum class NAV_SYS_MODE : unsigned char
+{
+  inertial = 1,
+  gps = 2,
+  Optical_flow = 3
+};
+enum class EXEC_CODE_COMMAND : unsigned char
+{
+  error = 0,
+  in_progress = 1,
+  completed = 2
+};
+enum class FEASIBILITY_CODE_COMMAND : unsigned char
+{
+  cannot_be_performed = 0,
+  accepted = 1
+};
+enum class DATA_TYPES : unsigned char
+{
+  dt_char = 0,
+  dt_unsigned_char = 1,
+  dt_string = 2,
+  dt_short = 3,
+  dt_bool = 4,
+  dt_unsigned_short = 5,
+  dt_int = 6,
+  dt_unsigned_int = 7,
+  dt_long = 8,
+  dt_unsigned_long = 9,
+  dt_long_long = 10,
+  dt_unsigned_long_long = 11,
+  dt_float_32 = 12,
+  dt_float_64 = 13,
+  dt_unknown = 255
+};
+
+//extern CRITICAL_SECTION ProtoSection;
+
+#pragma pack(push,1)
+
+struct MissionItemData
+{
+  unsigned long long item_number;
+  COMMANDS_ID item_command; // ID команды, может быть: движение к точке, возврат домой, посадка, смена скорости и др.
+  char param1; // резерв
+  float param2; // значение скорости, если в миссии есть команда смены скорости, для других команд - резерв
+  char param3; // резерв
+  float param4; // yaw
+  double param5; // широта
+  double param6; // долгота
+  float param7; // абс. высота
+};
+
+struct MissionMetaData
+{
+  unsigned long long current_item;
+  unsigned long long total_items;
+  bool launch_confidentiality;
+  bool mission_priority;
+  bool fault_tolerence;
+  bool f_start;
+  bool f_pause;
+  unsigned char mission_name_len;
+  char mission_name[MAX_LEN_MISSION_NAME];
+};
+struct MissionItemsData
+{
+  MissionItemData items[MAX_COUNT_MISSION_POINT];
+};
+
+struct EnginePowerInfo
+{
+  unsigned char power; //Текущая нагрузка на мотор от 0 до 100%
+  unsigned int engineSpeed;//Текущие оборты в секунду мотора
+  float current; // Ток потребляемый мотором
+  float voltage; // Напряжение на моторе
+  float temp; // Температура мотора
+
+  static const unsigned char MaxCount = 8;
+};
+
+struct ProtoDataBattery
+{
+  unsigned char perCharge;
+  float voltage;
+  float amperage;
+  float temp;
+  float totalPower;
+  unsigned long long timeRemaining;
+};
+struct ProtoDataSysInfo
+{
+  SYS_MODE mode;
+  STATUS_MODE statusMode;
+  NAV_SYS_MODE navSys;
+  ENGINE_STATUS engineStatus;
+  unsigned char engineCount;
+  float pressureBaro;
+  float tempBaro;
+
+  EnginePowerInfo enginePower[EnginePowerInfo::MaxCount];
+};
+struct ProtoDataGyroInfo
+{
+  float yawGyroVel;
+  float pitchGyroVel;
+  float rollGyroVel;
+};
+struct ProtoDataAccelInfo
+{
+  float yawAccelVel; 
+  float pitchAccelVel; 
+  float rollAccelVel; 
+  float aX; 
+  float aY; 
+  float aZ; 
+  float tempAccel; 
+};
+struct ProtoDataGpsInfo
+{
+  double lat;
+  double lon;
+  float absAlt;
+  float realAlt;
+  float hdop;
+  float vdop;
+  float pdop;
+  float noise;
+  float jamming;
+  unsigned char satVisible;
+  unsigned char satUsed;
+  float speed;
+  unsigned char fixType;
+  unsigned long long timeUTC;
+};
+struct ProtoDataInertialInfo
+{
+  float x;
+  float y;
+  float z;
+  float headingDeg;
+  float speed;
+  float roll;
+  float pitch;
+  float yaw;
+  float rollVel;
+  float pitchVel;
+  float yawVel;
+  float baroAlt;
+};
+
+struct ProtoDataRawCalib
+{
+  SENSOR_ID sensor;
+  float X;
+  float Y;
+  float Z;
+};
+
+struct ParameterData
+{
+  const char* name;
+  void* value;
+  DATA_TYPES type;
+};
+
+#pragma pack(pop)
+
+typedef void (*SendCallback)(void* data, unsigned long size);
+
+void MainInit(SendCallback send);
+bool MainUpdate(unsigned char byte);
+
+void ProtoDataWriteReadBattery(ProtoDataBattery& data, bool read=true);
+void ProtoDataWriteReadSysInfo(ProtoDataSysInfo& data, bool read = true);
+//void ProtoDataWriteReadEngineInfo(EnginePowerInfo data[], unsigned char count, bool read = true);
+void ProtoDataWriteReadGyroInfo(ProtoDataGyroInfo& data, bool read = true);
+void ProtoDataWriteReadAccelInfo(ProtoDataAccelInfo& data, bool read = true);
+void ProtoDataWriteReadGpsInfo(ProtoDataGpsInfo& data, bool read = true);
+void ProtoDataWriteReadInertialInfo(ProtoDataInertialInfo& data, bool read = true);
+void ProtoDataWriteReadRawCalib(ProtoDataRawCalib& data, bool read = true);
+
+void RawCalibData();
+
+extern MissionMetaData ProtoMissionInfo;
+extern MissionItemsData ProtoMissionData;
+
+#endif
\ No newline at end of file
diff --git a/Source/Drivers/Tick.cpp b/Source/Drivers/Tick.cpp
new file mode 100644
index 0000000..17cee3b
--- /dev/null
+++ b/Source/Drivers/Tick.cpp
@@ -0,0 +1,60 @@
+#include "Tick.h"
+#include "stm32g4xx.h"
+
+static unsigned long Tick = 0;
+
+extern "C" void SysTick_Handler()
+{
+  Tick++;
+}
+
+void TICK_Init()
+{
+  SysTick->LOAD = SystemCoreClock / 1000;
+  SysTick->VAL = 0UL;
+  SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_TICKINT_Msk | SysTick_CTRL_ENABLE_Msk;
+
+  NVIC_SetPriority(SysTick_IRQn, 0);
+}
+
+unsigned long TICK_GetCount() // ms
+{
+  return Tick;
+}
+
+float TICK_GetTime() // s
+{
+  return ((float)Tick)/1000.0f;
+}
+
+void TICK_Delay(unsigned long us)
+{
+  long tick=SysTick->VAL;
+  long wait=us*(SystemCoreClock/1000000);
+  
+  long load = SysTick->LOAD;
+  
+  long val;
+  
+  do
+  {
+    val=SysTick->VAL;
+    
+    if(val>tick) val-=load;
+    
+  } while(tick-val < wait);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/Source/Drivers/Tick.h b/Source/Drivers/Tick.h
new file mode 100644
index 0000000..dd0d476
--- /dev/null
+++ b/Source/Drivers/Tick.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#ifndef TICK_H
+#define TICK_H
+
+void TICK_Init();
+unsigned long TICK_GetCount();
+float TICK_GetTime();
+void TICK_Delay(unsigned long us); // 900 us maximum
+
+#endif
\ No newline at end of file
diff --git a/Source/Drivers/Tim.cpp b/Source/Drivers/Tim.cpp
new file mode 100644
index 0000000..b53fd1a
--- /dev/null
+++ b/Source/Drivers/Tim.cpp
@@ -0,0 +1,69 @@
+#include "Tim.h"
+#include "stm32g4xx.h"
+
+static ProcTIM TIM6_Proc1 = 0;
+static ProcTIM TIM6_Proc2 = 0;
+static ProcTIM TIM7_Proc  = 0;
+
+extern "C" void TIM6_DAC_IRQHandler()
+{
+  static int proc=0;
+  TIM6->SR = 0;
+  
+  
+  if(proc>3) 
+  {
+    TIM6_Proc2();
+    proc=0;
+  }
+  else proc++;
+  
+  TIM6_Proc1();
+}
+
+extern "C" void TIM7_IRQHandler()
+{
+  TIM7->SR = 0;
+  TIM7_Proc();
+}
+
+void TIM6_Init(long Priority, unsigned long Freq, const ProcTIM& Proc1, const ProcTIM& Proc2)
+{
+  RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
+
+  TIM6->CR1 = 0;
+  TIM6->ARR = 1000 - 1;
+  TIM6->PSC = (SystemCoreClock / 1000 / Freq) - 1;
+  TIM6->DIER = TIM_DIER_UIE;
+
+  TIM6_Proc1 = Proc1;
+  TIM6_Proc2 = Proc2;
+  
+  NVIC_SetPriority(TIM6_DAC_IRQn, Priority);
+  NVIC_EnableIRQ(TIM6_DAC_IRQn);
+}
+
+void TIM7_Init(long Priority, unsigned long Freq, const ProcTIM& Proc)
+{
+  RCC->APB1ENR1 |= RCC_APB1ENR1_TIM7EN;
+
+  TIM7->CR1 = 0;
+  TIM7->ARR = 1000 - 1;
+  TIM7->PSC = (SystemCoreClock / 1000 / Freq) - 1;
+  TIM7->DIER = TIM_DIER_UIE;
+
+  TIM7_Proc = Proc;
+  
+  NVIC_SetPriority(TIM7_IRQn, Priority);
+  NVIC_EnableIRQ(TIM7_IRQn);
+}
+
+void TIM6_Enable()
+{
+  TIM6->CR1 = TIM_CR1_CEN;
+}
+
+void TIM7_Enable()
+{
+  TIM7->CR1 = TIM_CR1_CEN;
+}
\ No newline at end of file
diff --git a/Source/Drivers/Tim.h b/Source/Drivers/Tim.h
new file mode 100644
index 0000000..ed63cf3
--- /dev/null
+++ b/Source/Drivers/Tim.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#ifndef TIM_H
+#define TIM_H
+
+typedef void (*ProcTIM)();
+
+void TIM6_Init(long Priority, unsigned long Freq, const ProcTIM& Proc1, const ProcTIM& Proc2);
+void TIM7_Init(long Priority, unsigned long Freq, const ProcTIM& Proc);
+
+void TIM6_Enable();
+void TIM7_Enable();
+
+#endif
\ No newline at end of file
diff --git a/Source/main.cpp b/Source/main.cpp
index 9aa7411..75e98f1 100644
--- a/Source/main.cpp
+++ b/Source/main.cpp
@@ -1,12 +1,111 @@
 #include "stm32g4xx.h"
+#include "Tick.h"
+#include "Tim.h"
+#include "IIMU.h"
 #include "ICM20948.h"
+#include "IRS.h"
+#include "Vector.h"
+#include "Autopilot.h"
 
 extern "C" void SystemClock_Config();
 
+constexpr long TIM_PRIORITY = 2;
+
+IMU_Data DataIMU;
+
+IRS MainIRS;
+
+IIMU* IMUObj;
+
+float GetCurrentTime()
+{
+  return ((float)TICK_GetCount()) / 1000.0f;
+}
+
+void DoneProcIMU(IMU_Data& Data)
+{
+  DataIMU = Data;
+  // MagReady=true;
+}
+
+void TimerUpdateSensor()
+{
+  //IMUObj->GetAsync(DoneProcIMU);
+  if(MainDrone.IMUInit) IMUObj->GetAsync(DoneProcIMU);
+  //if(MainDrone.BarInit) BarObj->GetAsync(DoneProcBAR);
+}
+
+void TimerUpdateMain()
+{
+  float time = GetCurrentTime();
+  
+  Vector3 gyr = {(float)DataIMU.Gyr.X, (float)DataIMU.Gyr.Y, (float)DataIMU.Gyr.Z};
+  Vector3 acc = {(float)DataIMU.Acc.X, (float)DataIMU.Acc.Y, (float)DataIMU.Acc.Z};
+  
+  // Vector3 mag;
+  // if(MainDrone.ExternMagInit) mag = {(float)DataMAG.X, (float)DataMAG.Y, (float)DataMAG.Z};
+  // else mag = {(float)DataIMU.Mag.X, (float)DataIMU.Mag.Y, (float)DataIMU.Mag.Z};
+  
+  MainIRS.UpdateGyro(gyr);
+  MainIRS.UpdateAccel(acc);
+  /*if(MagReady)
+  {
+    MagReady=false;
+    MainIRS.UpdateMagnet(mag);
+  }
+  
+  MainGPS.UpdateAverage(time);
+  MainBar.UpdateAverage();
+  MainOF.UpdateAverage(IRS::Period);
+  MainLen.UpdateAverage();
+  
+  if(DataFLOW.Update)
+  {
+    DataFLOW.Update=false;
+    UpdateFlow(DataFLOW.OF, MainIRS.Inertial.Pos.Z - MainIRS.GroundShift, DataFLOW.Valid, DataFLOW.Quality);
+  }
+  
+  if(DataTOF.Update)
+  {
+    DataTOF.Update=false;
+    UpdateRange(DataTOF.Range, DataTOF.Valid && DataTOF.Range>0.02f && DataTOF.Strength>200.0f);
+  }
+  
+  UpdateBar(DataBAR.Pressure, DataBAR.Temp, DataBAR.Pressure>0.0f);
+  
+  if(DataGPS.Update)
+  {
+    DataGPS.Update=false;
+    Vector3 gps = { DataGPS.X, DataGPS.Y, DataGPS.Z };
+    UpdateGPS(gps, time, DataGPS.Valid);
+  }*/
+  
+  Vector3 pos;
+  MainIRS.RestoreAllShift(pos);
+  /*MainVel.PointBegin-=pos;
+  
+  AutoControl(gyr);
+  
+  ReadStateINS();
+  
+  if(MainDrone.ExternMagInit) MagObj->GetAsync(DoneProcMag);*/
+}
+
 int main()
 {
   SystemClock_Config(); // 170MHz
   IMU_InitPower();
   
-  return 0;
+  TICK_Init();
+  
+  TIM6_Init(TIM_PRIORITY, 500, TimerUpdateSensor, TimerUpdateMain);
+  
+  IMUObj = TryFindIMU(MainDrone.IMUInit);
+  
+  TIM6_Enable();
+  
+  while (true)
+  {
+    
+  }
 }
\ No newline at end of file