add firmware

2025-07-28 12:43:33 +03:00
parent 6cf2747ec9
commit 748830dfb7
84 changed files with 40709 additions and 0 deletions
--- a/dev/bar.cpp
+++ b/dev/bar.cpp
@ -0,0 +1,85 @@
+#include <math.h>
+
+#include "i2c.h"
+
+#include "bar.h"
+
+static const unsigned char BAR_Addr = 0x5C; // LPS22HH
+
+void (*BAR_DoneProc)(BAR_Data& Data);
+
+float BAR_GetAltitude(float p0, float p1)
+{
+  return 44330.0f*(1.0f-powf(p1/p0, 1.0f/5.255f));
+}
+//------------------------------------------------------------------------------
+
+static inline void BAR_SetReg(unsigned char Reg, unsigned char Value)
+{
+  unsigned char reg[2];
+  reg[0]=Reg; reg[1]=Value;
+  I2C2_Write(BAR_Addr, reg, 2);
+  //I2C2_Stop();
+}
+//------------------------------------------------------------------------------
+
+void BAR_Init()
+{
+  I2C2_Init();
+
+  BAR_SetReg(0x10, 0x3E); // RESET
+  
+  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
+}
+//------------------------------------------------------------------------------
+
+float BAR_GetData(float* Temp)
+{
+  static float bar=0;
+  static float temp=0;
+  
+  unsigned char st;
+  I2C2_Write(BAR_Addr, 0x27);
+  I2C2_Read(BAR_Addr, &st, 1);
+  I2C2_Stop();
+  
+  if(st & 1)
+  {
+    unsigned char reg[3];
+    I2C2_Write(BAR_Addr, 0x28);
+    I2C2_Read(BAR_Addr, reg, sizeof(reg));
+    I2C2_Stop();
+    
+
+    unsigned long b;
+
+    b = reg[2];
+    b = (b * 256U) + reg[1];
+    b = (b * 256U) + reg[0];
+    b *= 256U;
+    
+    bar=((float)b)/1048576.0f;
+  }
+  
+  if(st & 2)
+  {
+    unsigned char reg[2];
+    I2C2_Write(BAR_Addr, 0x2B);
+    I2C2_Read(BAR_Addr, reg, sizeof(reg));
+   I2C2_Stop();
+    
+
+    short t;
+
+    t = (short)reg[1];
+    t = (t * 256) + (short)reg[0];
+    
+    temp = (float) t / 100.0f;
+  }
+  
+  *Temp=temp;
+  
+  return bar;
+  
+}
+//------------------------------------------------------------------------------
--- a/dev/bar.h
+++ b/dev/bar.h
@ -0,0 +1,13 @@
+#pragma once
+
+struct BAR_Data
+{
+  long Temp;
+  float Pressure;
+};
+
+void BAR_Init();
+
+float BAR_GetData(float* Temp);
+void BAR_GetAsunc(void (*DoneProc)(BAR_Data& Data));
+float BAR_GetAltitude(float p0, float p1);
--- a/dev/com.cpp
+++ b/dev/com.cpp
@ -0,0 +1,97 @@
+#include <string.h>
+
+#include "uart.h"
+#include "tick.h"
+
+#include "com.h"
+
+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,
+    
+  CountMenuCategories = 32,
+  CategoriesMenu = 33,
+  CategoriesParameters = 34,
+};
+
+enum class COMMANDS_NAME : unsigned char
+{
+  ChangeNav = 1,
+  ChangeSpeed = 2,
+  Land = 3,
+  GoHome = 4,
+  StopEngine = 5,
+  StartEngine = 6,
+  Pause = 7,
+  Continue = 8,
+  GoToGlobal = 9,
+  GoToLocal = 10,
+  
+  SetParameter = 15,
+};
+
+enum class ERROR_CODE_COMMAND : unsigned char
+{
+  NoError = 0,
+};
+
+#pragma pack(push,1)
+
+struct HeaderBegin
+{
+  unsigned char stx = 0xAA;
+  unsigned short len1;
+  unsigned short len2;
+  unsigned char crc;
+  unsigned char data[0];
+  
+  bool CheckCRC()
+  {
+    if(len1!=len2) return false;
+    unsigned char test = 0;
+    for (unsigned short a = 0; a < len1; a++) test ^= data[a];
+    return crc==test;
+  }
+};
+
+struct HeaderMessages
+{
+  MESSAGES_ID msgId;
+  unsigned char srcId;
+  unsigned char dstId;
+  unsigned char len;
+  unsigned char data[0];
+  
+  
+};
+
+#pragma pack(pop)
+
+//void TELE_Init()
+//{
+//  UART2_Init(57600);
+//}
+//
+//void TELE_Update(const void* info, unsigned long size, unsigned long update)
+//{
+//  
+//}
--- a/dev/com.h
+++ b/dev/com.h
@ -0,0 +1,4 @@
+#pragma once
+
+void COM_Init();
+void COM_Update();
--- a/dev/eep.cpp
+++ b/dev/eep.cpp
@ -0,0 +1,35 @@
+#include "i2c.h"
+
+#include "eep.h"
+
+static const unsigned char EEP_Addr = 0x50; // AT24C256
+
+static inline short Rev16(short v)
+{
+  asm("REV16 %1, %0" : "=r" (v) : "r" (v)); // v = v<<8 | v>>8;
+  return v;
+}
+//------------------------------------------------------------------------------
+
+void EEP_Init() // AT24C256
+{
+  I2C1_Init();
+}
+//------------------------------------------------------------------------------
+
+void EEP_Read(unsigned short Addr, void* Data, unsigned short Size)
+{
+  Addr=Rev16(Addr);
+  I2C1_Write(EEP_Addr, &Addr, 2);
+  I2C1_Read(EEP_Addr, Data, Size);
+  I2C1_Stop();
+}
+//------------------------------------------------------------------------------
+
+void EEP_Write(unsigned short Addr, const void* Data, unsigned short Size)
+{
+  Addr=Rev16(Addr);
+  I2C1_Write2(EEP_Addr, &Addr, 2, Data, Size);
+  I2C1_Stop();
+}
+//------------------------------------------------------------------------------
--- a/dev/eep.h
+++ b/dev/eep.h
@ -0,0 +1,6 @@
+#pragma once
+
+void EEP_Init();
+
+void EEP_Read(unsigned short Addr, void* Data, unsigned short Size);
+void EEP_Write(unsigned short Addr, const void* Data, unsigned short Size);
--- a/dev/flow.cpp
+++ b/dev/flow.cpp
@ -0,0 +1,131 @@
+#include <string.h>
+#include <stdlib.h>
+
+#include "gpio.h"
+
+#include "spi.h"
+#include "tick.h"
+
+#include "laser.h"
+
+static void WriteReg(char reg, char value)
+{
+  char send[2]={reg | 0x80, value};
+  SPI2_TransferCons(send, 2, 0, 0);
+}
+
+bool FLOW_Init() // PMW3901
+{
+  SPI2_Init();
+  
+  WriteReg(0x3A, 0x5A);
+  
+  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
+
+  // Test the SPI communication, checking chipId and inverse chipId
+  
+  char reg[4]={0x00, 0, 0x5F, 0};
+   char result[4]={0, 0,0,0};
+  SPI2_TransferParallel(reg, result, 4);
+  
+  if (result[1] != 0x49 || result[3] != 0xB6) return false;
+  
+  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
+  
+  WriteReg(0x7F, 0x00);
+  WriteReg(0x61, 0xAD);
+  WriteReg(0x7F, 0x03);
+  WriteReg(0x40, 0x00);
+  WriteReg(0x7F, 0x05);
+  WriteReg(0x41, 0xB3);
+  WriteReg(0x43, 0xF1);
+  WriteReg(0x45, 0x14);
+  WriteReg(0x5B, 0x32);
+  WriteReg(0x5F, 0x34);
+  WriteReg(0x7B, 0x08);
+  WriteReg(0x7F, 0x06);
+  WriteReg(0x44, 0x1B);
+  WriteReg(0x40, 0xBF);
+  WriteReg(0x4E, 0x3F);
+  
+  WriteReg(0x7F, 0x08);
+  WriteReg(0x65, 0x20);
+  WriteReg(0x6A, 0x18);
+  WriteReg(0x7F, 0x09);
+  WriteReg(0x4F, 0xAF);
+  WriteReg(0x5F, 0x40);
+  WriteReg(0x48, 0x80);
+  WriteReg(0x49, 0x80);
+  WriteReg(0x57, 0x77);
+  WriteReg(0x60, 0x78);
+  WriteReg(0x61, 0x78);
+  WriteReg(0x62, 0x08);
+  WriteReg(0x63, 0x50);
+  WriteReg(0x7F, 0x0A);
+  WriteReg(0x45, 0x60);
+  WriteReg(0x7F, 0x00);
+  WriteReg(0x4D, 0x11);
+  WriteReg(0x55, 0x80);
+  WriteReg(0x74, 0x1F);
+  WriteReg(0x75, 0x1F);
+  WriteReg(0x4A, 0x78);
+  WriteReg(0x4B, 0x78);
+  WriteReg(0x44, 0x08);
+  WriteReg(0x45, 0x50);
+  WriteReg(0x64, 0xFF);
+  WriteReg(0x65, 0x1F);
+  WriteReg(0x7F, 0x14);
+  WriteReg(0x65, 0x60);
+  WriteReg(0x66, 0x08);
+  WriteReg(0x63, 0x78);
+  WriteReg(0x7F, 0x15);
+  WriteReg(0x48, 0x58);
+  WriteReg(0x7F, 0x07);
+  WriteReg(0x41, 0x0D);
+  WriteReg(0x43, 0x14);
+  WriteReg(0x4B, 0x0E);
+  WriteReg(0x45, 0x0F);
+  WriteReg(0x44, 0x42);
+  WriteReg(0x4C, 0x80);
+  WriteReg(0x7F, 0x10);
+  WriteReg(0x5B, 0x02);
+  WriteReg(0x7F, 0x07);
+  WriteReg(0x40, 0x41);
+  WriteReg(0x70, 0x00);
+  
+  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
+  
+  WriteReg(0x32, 0x44);
+  WriteReg(0x7F, 0x07);
+  WriteReg(0x40, 0x40);
+  WriteReg(0x7F, 0x06);
+  WriteReg(0x62, 0xf0);
+  WriteReg(0x63, 0x00);
+  WriteReg(0x7F, 0x0D);
+  WriteReg(0x48, 0xC0);
+  WriteReg(0x6F, 0xd5);
+  WriteReg(0x7F, 0x00);
+  WriteReg(0x5B, 0xa0);
+  WriteReg(0x4E, 0xA8);
+  WriteReg(0x5A, 0x50);
+  WriteReg(0x40, 0x80);
+  
+  return true;
+}
+
+bool FLOW_GetMotion(short* dX, short* dY, unsigned char* qual)
+{
+  char reg[12]={0x02, 0, 0x03, 0, 0x04, 0, 0x05, 0, 0x06, 0, 0x07, 0};
+  
+  SPI2_TransferParallel(reg, reg, 12);
+  
+  short x = ((short)reg[5] << 8) | reg[3];
+  short y = ((short)reg[9] << 8) | reg[7];
+  
+  if(dX) *dX = x;
+  if(dY) *dY = y;
+  
+  if(qual) *qual = reg[11];
+  
+  return reg[1] & 0x80;
+}
--- a/dev/flow.h
+++ b/dev/flow.h
@ -0,0 +1,4 @@
+#pragma once
+
+bool FLOW_Init();
+bool FLOW_GetMotion(short* dX, short* dY, unsigned char* qual);
--- a/dev/gps.cpp
+++ b/dev/gps.cpp
@ -0,0 +1,568 @@
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include "gpio.h"
+
+#include "uart.h"
+#include "tick.h"
+#include "ori.h"
+
+#include "filt.h"
+
+#include "gps.h"
+
+//FilterGPS filt_x, filt_y, filt_z;
+
+/*void GPS_Navigation(bool valid, ORI_Data& data, float p[3])
+{
+  filt_x.Update(valid, p[0], data.Iner.X/1000);
+  filt_y.Update(valid, p[1], data.Iner.Y/1000);
+  filt_z.Update(valid, p[2], data.Iner.Z/1000);
+  
+  data.Speed.X=filt_x.Speed.Value;
+  data.Speed.Y=filt_y.Speed.Value;
+  data.Speed.Z=filt_z.Speed.Value;
+  
+  data.Pos.X=filt_x.Position.Value;
+  data.Pos.Y=filt_y.Position.Value;
+  data.Pos.Z=filt_z.Position.Value;
+}*/
+//------------------------------------------------------------------------------
+
+float GPS_LocalDistance(Point p1, Point p2, float& dx, float& dy) // light formula
+{
+  const float pi = 3.14159265359f;
+  const float er = 6371000.0f; // Radius of the earth in m
+
+  float lat = p1.Latitude - p2.Latitude;
+  float lon = p1.Longitude - p2.Longitude;
+
+  float y = er/360.0f * (lat*pi*2.0f); // lat
+  dy = y;
+
+  float l = ((float)(p1.Latitude + p2.Latitude)) / 2.0f;
+  float r = cosf(l*pi/180.0f) * er;
+  float x = r/360.0f * (lon*pi*2.0f); // long
+  dx = x;
+
+  float d = sqrtf(x*x + y*y);
+  return d;
+}
+//------------------------------------------------------------------------------
+
+float GPS_GlobalDistance(Point p1, Point p2) // Haversine formula
+{
+  const float pi = 3.14159265359f;
+  const float er = 6371000.0f; // Radius of the earth in m
+
+  float dLat = ((float)(p2.Latitude - p1.Latitude))*pi/180.0f;
+  float dLon = ((float)(p2.Longitude - p1.Longitude))*pi/180.0f;
+
+  float lat1 = p1.Latitude, lat2 = p2.Latitude;
+
+  float sdlat=sinf(dLat/2.0f);
+  float sdlon=sinf(dLon/2.0f);
+  
+  float a = (sdlat*sdlat) + cosf((lat1)*pi/180.0f) * cosf((lat2)*pi/180.0f) * (sdlon*sdlon);
+  float c = 2.0f * atan2f(sqrtf(a), sqrtf(1.0f-a));
+  float d = er * c; // Distance in m
+  return d;
+}
+//------------------------------------------------------------------------------
+
+struct GPS_Head
+{
+  static const unsigned long Size = 7;
+  const char* NMEA;
+};
+
+struct GPS_GNRMC // $GNRMC,hhmmss.sss,A,ggmm.mm,P,gggmm.mm,J,v.v,b.b,ddmmyy,x.x,n,m*hh/r/n
+{
+  static constexpr GPS_Head Head = {"$GNRMC,"}; // Recommended minimum specific Transit data
+  //---
+  char* Time;      // UTC of position fix
+  char* Valid;     // Data status (V=navigation receiver warning)
+  char* Latitude;  // Latitude of fix
+  char* Pole;      // N or S
+  char* Longitude; // Longitude of fix
+  char* J;         // E or W
+  char* HorSpeed;  // Speed over ground in knots
+  char* Angle;     // Track made good in degrees True
+  char* Date;      // UT date
+  char* Mag;       // Magnetic variation degrees (Easterly var. subtracts from true course)
+  char* MagAng;    // Magnetic declination direction, E (east) or W (west)
+  char* Mode;      // Mode indication (A=autonomous positioning, D=differential, E=estimation, N=invalid data)
+  //---
+  // Checksum
+};
+
+struct GPS_GNGGA // $GNGGA,hhmmss.ss,ggmm.mm,a,gggmm.mm,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh/r/n
+{
+  static constexpr GPS_Head Head = {"$GNGGA,"}; // Global Positioning System Fix Data
+  //---
+  char* Time;          // UTC of position
+  char* Latitude;      // latitude of position
+  char* Pole;          // N or S
+  char* Longitude;     // Longitude of position
+  char* J;             // E or W
+  char* Quality;       // GPS Quality indicator (0=no fix, 1=GPS fix, 2=Dif. GPS fix)
+  char* Satellites;    // Number of satellites in use [not those in view]
+  char* HDOP;          // Horizontal dilution of position
+  char* Altitude;      // Antenna altitude above/below mean sea level (geoid)
+  char* UnitsAlt;      // Meters  (Antenna height unit)
+  char* Geoidal;       // Geoidal separation (Diff. between WGS-84 earth ellipsoid andmean sea level.  -=geoid is below WGS-84 ellipsoid)
+  char* UnitsGeoidal;  // Meters  (Units of geoidal separation)
+  char* LastUpdate;    // Age in seconds since last update from diff. reference station
+  char* StationID;     // Diff. reference station ID#
+  //---
+  // Checksum
+};
+
+struct GPS_GNGSA // $GNGSA,mode,fix,PRN(11),PDOP,HDOP,VDOP,*hh/r/n
+{
+  static constexpr GPS_Head Head = {"$GNGSA,"}; // Global Positioning System Fix Data
+  //---
+  char* Mode;    // M = Manual, A = Automatic
+  char* FixType; // 1 = not available, 2 = 2D, 3 = 3D
+  char* ID_1;    // 01 to 32 for GPS, 33 to 64 for SBAS, 64+ for GLONASS
+  char* ID_2;
+  char* ID_3;
+  char* ID_4;
+  char* ID_5;
+  char* ID_6;
+  char* ID_7;
+  char* ID_8;
+  char* ID_9;
+  char* ID_10;
+  char* ID_11;
+  char* ID_12;
+  char* PDOP;
+  char* HDOP;
+  char* VDOP;
+  //---
+  // Checksum
+};
+
+struct GPS_GPGSV // $GPGSV,count,index,visible,[ID,elevation,azimuth,SNR]*hh/r/n
+{
+  static constexpr GPS_Head Head = {"$GPGSV,"}; // Global Positioning System Fix Data
+  //---
+  char* Count;    // M = Manual, A = Automatic
+  char* Index;   // 1 = not available, 2 = 2D, 3 = 3D
+  char* Visible; // 01 to 32 for GPS, 33 to 64 for SBAS, 64+ for GLONASS
+  
+  char* ID_1;
+  char* Elevation_1;
+  char* Azimuth_1;
+  char* SNR_1;
+  
+  char* ID_2;
+  char* Elevation_2;
+  char* Azimuth_2;
+  char* SNR_2;
+  
+  char* ID_3;
+  char* Elevation_3;
+  char* Azimuth_3;
+  char* SNR_3;
+  
+  char* ID_4;
+  char* Elevation_4;
+  char* Azimuth_4;
+  char* SNR_4;
+  
+  //---
+  // Checksum
+};
+
+struct GPS_GLGSV // $GLGSV,count,index,visible,[ID,elevation,azimuth,SNR]*hh/r/n
+{
+  static constexpr GPS_Head Head = {"$GLGSV,"}; // Global Positioning System Fix Data
+  //---
+  char* Count;    // M = Manual, A = Automatic
+  char* Index;   // 1 = not available, 2 = 2D, 3 = 3D
+  char* Visible; // 01 to 32 for GPS, 33 to 64 for SBAS, 64+ for GLONASS
+  
+  char* ID_1;
+  char* Elevation_1;
+  char* Azimuth_1;
+  char* SNR_1;
+  
+  char* ID_2;
+  char* Elevation_2;
+  char* Azimuth_2;
+  char* SNR_2;
+  
+  char* ID_3;
+  char* Elevation_3;
+  char* Azimuth_3;
+  char* SNR_3;
+  
+  char* ID_4;
+  char* Elevation_4;
+  char* Azimuth_4;
+  char* SNR_4;
+  
+  //---
+  // Checksum
+};
+
+enum class GPS_PROTO {GNRMC, GNGGA, GNGSA, GPGSV, GLGSV, COUNT};
+static const long GPS_ProtCount = (long)GPS_PROTO::COUNT;
+static GPS_Head GPS_Protocols[GPS_ProtCount] = { GPS_GNRMC::Head, GPS_GNGGA::Head, GPS_GNGSA::Head, GPS_GPGSV::Head, GPS_GLGSV::Head };
+
+struct GPS_Info
+{
+  GPS_PROTO Type;
+
+  static const unsigned long Length = 256;
+  char Data[Length];
+
+  static const unsigned long Count = 32;
+  char* Param[Count];
+
+  long Begin = 0;
+  long Index = 0;
+  long Size = 0;
+
+  unsigned char CRC8;
+};
+
+
+static GPS_Info Info;
+
+void (*GPS_CallBack)(GPS_PROTO Proto, void* Data)=0;
+
+//------------------------------------------------------------------------------------------------------------------------------
+/*static unsigned long UARTt_Recv(void* Data, unsigned long Size, bool WaitAll=false) // For Check ONLY !!!!!!!!!!!!!!!!
+{
+  static int begin=0;
+  
+  const char test[]="$GNGGA,122013.10,4719.45110,N,03846.54105,E,1,12,0.66,35.9,M,17.6,M,,*7A\r\n";
+  unsigned long size=strlen(test);
+  
+  memcpy(Data, test, Size<size ? Size : size);
+  //memcpy(Data, test+begin, 1);
+  
+  begin++;
+  
+  if(begin>=size) begin=0;
+  
+  //return 1;
+  return size;
+}*/
+//------------------------------------------------------------------------------------------------------------------------------
+
+void GPS_Update()
+{
+  char* head = Info.Data;
+
+  Info.Size += UART1_Recv(head + Info.Size, Info.Length - Info.Size);
+
+  long size = Info.Size - Info.Index;
+
+  while (size)
+  {
+    char* data = head + Info.Index;
+  
+    if (Info.Index && *data == '$')
+    {
+      memcpy(head, data, size);
+      Info.Index = 0;
+      Info.Begin = 0;
+      Info.Size = size;
+      data = head;
+    }
+  
+    if (!Info.Begin)
+    {
+      if (*head != '$')
+      {
+        Info.Index++;
+        size--;
+        continue;
+      }
+      //---
+      if (Info.Size < GPS_Head::Size)  return;
+      //---
+      Info.Type = GPS_PROTO::COUNT;
+      for (long a = 0; a < GPS_ProtCount; a++)
+      {
+        const char* nmea = GPS_Protocols[a].NMEA;
+        if (memcmp(head, nmea, GPS_Head::Size) == 0)
+        {
+          Info.CRC8 = 0x0E;
+          for (long a = 0; nmea[a]; a++) Info.CRC8 ^= nmea[a];
+          Info.Type = (GPS_PROTO)a;
+          break;
+        }
+      }
+      //---
+      if (Info.Type == GPS_PROTO::COUNT)
+      {
+        Info.Index++;
+        *head = '#';
+        size--;
+        continue;
+      }
+      //---
+      Info.Index = GPS_Head::Size;
+      size = Info.Size - GPS_Head::Size;
+      //---
+      Info.Param[Info.Begin++] = head + Info.Index;
+      //---
+      continue;
+    }
+    //---
+    if (Info.Size > Info.Length)
+    {
+      Info.Begin = 0;
+      Info.Index = 0;
+      Info.Size = 0;
+      return;
+    }
+    //---
+    if (Info.Begin >= Info.Count)
+    {
+      if (*data == '\n')
+      {
+        *data = '\0';
+        size--;
+        char* end;
+        unsigned char crc8 = strtol(data-3, &end, 16);
+        if ((end == data - 1) && (crc8 == Info.CRC8))
+        {
+          if (GPS_CallBack) GPS_CallBack(Info.Type, Info.Param);
+          //---
+          if (size) memcpy(Info.Data, data + 1, size);
+        }
+        //---
+        Info.Begin = 0;
+        Info.Index = 0;
+        Info.Size = size;
+        //---
+        continue;
+      }
+    }
+    else
+    {
+      Info.CRC8 ^= *data;
+      //---
+      if (*data == ',')
+      {
+        *data = '\0';
+        Info.Param[Info.Begin++] = ++data;
+      }
+      else if (*data == '*')
+      {
+        *data++ = '\0';
+        while (Info.Begin < Info.Count)
+        {
+          Info.Param[Info.Begin++] = nullptr;
+        }
+      }
+    }
+    //---
+    Info.Index++;
+    size--;
+  }
+  
+  if (!Info.Begin && Info.Index)
+  {
+    Info.Index = 0;
+    Info.Size = 0;
+  }
+}
+//------------------------------------------------------------------------------
+
+static unsigned char GPS_VisibleGPS=0;
+static unsigned char GPS_VisibleGLO=0;
+
+static unsigned char Base_Noise[256];
+
+Point GetCoordinates_Coord;
+bool GetCoordinates_Valid;
+bool GetCoordinates_Update;
+
+Point Base_BeginXYZ;
+
+static float Base_Alt=0, Base_Geo=0;
+static unsigned char Base_Used, Base_Fix;
+
+static GPS_BaseInfo GPS_BaseInfoData;
+
+static void GPS_CallBack_GSV(GPS_PROTO Proto, void* Data)
+{
+  if(Proto==GPS_PROTO::GNGGA)
+  {
+    GetCoordinates_Update=true;
+    
+    GPS_GNGGA* data=(GPS_GNGGA*)Data;
+    
+    GetCoordinates_Valid = (data->Quality[0] && (data->Quality[0]=='1' || data->Quality[0]=='2'));
+    
+    float geo=0;
+    if(data->Geoidal[0] && GetCoordinates_Valid) geo=strtof(data->Geoidal, 0);
+    
+    if(data->Altitude[0]) GetCoordinates_Coord.Altitude=strtof(data->Altitude, 0);
+    //---
+    Base_Geo=GetCoordinates_Coord.Altitude+geo;
+    
+    
+    Base_Alt=Base_Geo-Base_BeginXYZ.Altitude;
+    //---
+    GetCoordinates_Coord.Altitude+=geo;
+    
+    if(data->Latitude[0] && GetCoordinates_Valid)
+    {
+      GetCoordinates_Coord.Latitude=(data->Latitude[0]-'0')*10 + data->Latitude[1]-'0';
+      GetCoordinates_Coord.Latitude+=strtod(data->Latitude+2, 0)/60;
+    }
+    //---
+    if(data->Longitude[0] && GetCoordinates_Valid)
+    {
+      GetCoordinates_Coord.Longitude=(data->Longitude[0]-'0')*100 + (data->Longitude[1]-'0')*10 + data->Longitude[2]-'0';
+      GetCoordinates_Coord.Longitude+=strtod(data->Longitude+3, 0)/60;
+    }
+    
+    if(data->Satellites[0] && GetCoordinates_Valid) Base_Used=strtoul(data->Satellites, 0, 10);
+    
+    if(data->Quality[0] && GetCoordinates_Valid) Base_Fix=strtoul(data->Quality, 0, 10);
+  }
+  
+  if(Proto==GPS_PROTO::GPGSV)
+  {
+    GPS_GPGSV* data=(GPS_GPGSV*)Data;
+    GPS_VisibleGPS = strtoul(data->Visible, 0, 10);
+    
+    if(!data->Index || *data->Index=='1') memset(Base_Noise, 0, sizeof(Base_Noise));
+    
+    long id;
+    
+    if(data->ID_1)
+    {
+      id=strtoul(data->ID_1, 0, 10);
+      if(id<256) Base_Noise[id]=strtoul(data->SNR_1, 0, 10);
+    }
+    if(data->ID_2)
+    {
+      id=strtoul(data->ID_2, 0, 10);
+      if(id<256) Base_Noise[id]=strtoul(data->SNR_2, 0, 10);
+    }
+    if(data->ID_3)
+    {
+      id=strtoul(data->ID_3, 0, 10);
+      if(id<256) Base_Noise[id]=strtoul(data->SNR_3, 0, 10);
+    }
+    if(data->ID_4)
+    {
+      id=strtoul(data->ID_4, 0, 10);
+      if(id<256) Base_Noise[id]=strtoul(data->SNR_4, 0, 10);
+    }
+  }
+  else if(Proto==GPS_PROTO::GLGSV)
+  {
+    GPS_GLGSV* data=(GPS_GLGSV*)Data;
+    GPS_VisibleGLO = strtoul(data->Visible, 0, 10);
+    
+    long id;
+    
+    if(data->ID_1)
+    {
+      id=strtoul(data->ID_1, 0, 10);
+      if(id<256) Base_Noise[id]=strtoul(data->SNR_1, 0, 10);
+    }
+    if(data->ID_2)
+    {
+      id=strtoul(data->ID_2, 0, 10);
+      if(id<256) Base_Noise[id]=strtoul(data->SNR_2, 0, 10);
+    }
+    if(data->ID_3)
+    {
+      id=strtoul(data->ID_3, 0, 10);
+      if(id<256) Base_Noise[id]=strtoul(data->SNR_3, 0, 10);
+    }
+    if(data->ID_4)
+    {
+      id=strtoul(data->ID_4, 0, 10);
+      if(id<256) Base_Noise[id]=strtoul(data->SNR_4, 0, 10);
+    }
+    
+    if(*data->Count==*data->Index)
+    {
+      float noise=0, count=0;
+      for(long a=0; a<256; a++)
+      {
+        if(!Base_Noise[a]) continue;
+        noise+=Base_Noise[a];
+        count++;  
+      }
+      
+      if(count) GPS_BaseInfoData.noise=noise/count;
+      else GPS_BaseInfoData.noise=0;
+      
+      GPS_BaseInfoData.satVisible=GPS_VisibleGPS+GPS_VisibleGLO;
+    }
+  }
+  else if(Proto==GPS_PROTO::GNRMC)
+  {
+    GPS_GNRMC* data=(GPS_GNRMC*)Data;
+    
+    GPS_BaseInfoData.lat=GetCoordinates_Coord.Latitude;
+    GPS_BaseInfoData.lon=GetCoordinates_Coord.Longitude;
+    
+    if(data->HorSpeed[0]) GPS_BaseInfoData.speed=strtof(data->HorSpeed, 0);
+    
+    if(data->Time[0]) GPS_BaseInfoData.timeUTC=strtof(data->Time, 0);
+    
+    float dx, dy;
+    GPS_LocalDistance(Base_BeginXYZ, {GPS_BaseInfoData.lat, GPS_BaseInfoData.lon}, dx, dy);
+                      
+    GPS_BaseInfoData.LocalXYZ[0]=dx;
+    GPS_BaseInfoData.LocalXYZ[1]=-dy;
+    GPS_BaseInfoData.LocalXYZ[2]=Base_Geo-Base_BeginXYZ.Altitude;
+  }
+  else if(Proto==GPS_PROTO::GNGSA)
+  {
+    GPS_GNGSA* data=(GPS_GNGSA*)Data;
+    
+    if(data->HDOP[0]) GPS_BaseInfoData.hdop=strtof(data->HDOP, 0);
+    if(data->VDOP[0]) GPS_BaseInfoData.vdop=strtof(data->VDOP, 0);
+    if(data->PDOP[0]) GPS_BaseInfoData.pdop=strtof(data->PDOP, 0);
+  }
+}
+//------------------------------------------------------------------------------
+
+void GPS_Init()
+{
+  UART1_Init(230400);
+  GPS_CallBack=GPS_CallBack_GSV;
+  memset(Base_Noise, 0, sizeof(Base_Noise));
+}
+
+bool GPS_GetCoordinates(Point& Coord, bool& Valid)
+{
+  if(!GetCoordinates_Update) return false;
+  
+  GetCoordinates_Update=false;
+  
+  Coord=GetCoordinates_Coord;
+  Valid=GetCoordinates_Valid;
+  
+  return true;
+}
+//------------------------------------------------------------------------------
+
+bool GPS_GetBaseInfo(GPS_BaseInfo& Info)
+{
+  Info=GPS_BaseInfoData;
+  
+  Info.realAlt=Base_Alt;
+  Info.absAlt=Base_Geo;
+  Info.satUsed=Base_Used;
+  Info.fixType=Base_Fix;
+  
+  return true;
+}
+//------------------------------------------------------------------------------
--- a/dev/gps.h
+++ b/dev/gps.h
@ -0,0 +1,35 @@
+#pragma once
+
+struct Point { double Latitude; double Longitude; float Altitude; };
+
+extern Point Base_BeginXYZ;
+
+void GPS_Navigation(bool valid, ORI_Data& data, float p[3]);
+
+float GPS_LocalDistance(Point p1, Point p2, float& dx, float& dy); // light formula
+float GPS_GlobalDistance(Point p1, Point p2); // Haversine formula
+
+void GPS_Init();
+void GPS_Update();
+bool GPS_GetCoordinates(Point& Coord, bool& Valid);
+
+struct GPS_BaseInfo
+{
+  float LocalXYZ[3];
+  float lat; 
+  float 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; // NO_GPS - 0, NO_FIX - 1, 2D_FIX - 2, 3D_FIX - 3, DGPS - 4, RTK_FLOAT - 5, RTK_FIXED - 6, STATIC - 7, PPP - 8
+  unsigned long long timeUTC;
+};
+
+bool GPS_GetBaseInfo(GPS_BaseInfo& Info);
--- a/dev/imu.cpp
+++ b/dev/imu.cpp
@ -0,0 +1,71 @@
+#include "i2c.h"
+
+#include "imu.h"
+
+static const unsigned char IMU_Addr = 0x6A; // ACC GYR
+
+static inline short Rev16(short v)
+{
+  asm("REV16 %1, %0" : "=r" (v) : "r" (v)); // v = v<<8 | v>>8;
+  return v;
+}
+//------------------------------------------------------------------------------
+
+static inline void IMU_SetReg(unsigned char Reg, unsigned char Value)
+{
+  unsigned char reg[2];
+  reg[0]=Reg; reg[1]=Value;
+  I2C2_Write(IMU_Addr, reg, 2);
+ I2C2_Stop();
+}
+//------------------------------------------------------------------------------
+
+static inline unsigned char IMU_GetReg(unsigned char Reg)
+{
+  I2C2_Write(IMU_Addr, Reg);
+  I2C2_Read(IMU_Addr, &Reg, 1);
+  I2C2_Stop();
+  return Reg;
+}
+//------------------------------------------------------------------------------
+
+void IMU_Init() 
+{ 
+  I2C2_Init();
+  
+  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
+
+  unsigned char wai=IMU_GetReg(0x0F);
+  
+   IMU_SetReg(0x10, 0x38);
+  IMU_SetReg(0x11, 0x48);
+  IMU_SetReg(0x12, 0x06);
+  
+
+  
+  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
+}
+//------------------------------------------------------------------------------
+
+void IMU_Get(IMU_Data& Data)
+{
+  struct {short temp; short gx; short gy; short gz; short ax; short ay; short az; } data;
+
+  I2C2_Write(IMU_Addr, 0x20);
+  I2C2_Read(IMU_Addr, &data, sizeof(data));
+  I2C2_Stop();
+
+  Data.Temp  = 21+(((long)Rev16(data.temp))*128)/42735; // 21+(temperature / 333.87)
+  
+  
+  //------------------------------ imu perevernuta, x z inverse
+  Data.Acc.X = -Rev16(data.ax);
+  Data.Acc.Y = Rev16(data.ay);
+  Data.Acc.Z = -Rev16(data.az);
+  
+  Data.Gyr.X = -Rev16(data.gx);
+  Data.Gyr.Y = Rev16(data.gy);
+  Data.Gyr.Z = -Rev16(data.gz);
+  
+}
+//------------------------------------------------------------------------------
--- a/dev/imu.h
+++ b/dev/imu.h
@ -0,0 +1,20 @@
+#pragma once
+
+struct IMU_Data
+{
+  short Temp;
+  struct { short X, Y, Z; } Acc;
+  struct { short X, Y, Z; } Gyr;
+  struct { short X, Y, Z; } Mag;
+  
+  float Bar;
+  float Tmp;
+};
+
+struct MAG_Data
+{
+  short X, Y, Z;
+};
+
+void IMU_Init();
+void IMU_Get(IMU_Data& Data);
--- a/dev/laser.cpp
+++ b/dev/laser.cpp
@ -0,0 +1,54 @@
+#include "laser.h"
+#include "i2c.h"
+#include <stdint.h>
+
+#include "vl53l0x.h"
+#include "tick.h"
+
+#define ADDR_LASER 0x29  // VL53L0X
+#define IDENTIFICATION_MODEL_ID            0xC0
+
+
+
+
+
+unsigned char Laser_ReadReg(unsigned char reg)
+{
+    unsigned char val;
+    I2C1_Write(ADDR_LASER, reg);
+    I2C1_Read(ADDR_LASER, &val, 1);
+    I2C1_Stop();
+    return val;
+}
+
+unsigned char Laser_WriteReg(unsigned char reg, unsigned char reg2)
+{
+    unsigned char val;
+    I2C1_Write(ADDR_LASER, reg);
+    
+    I2C1_Write(ADDR_LASER, reg2);
+    I2C1_Stop();
+    return val;
+}
+
+VL53L0X sensor;
+
+bool Laser_Init_v2(void){
+  I2C1_Init();
+  bool fl = sensor.init();
+  sensor.startContinuous();
+  return fl;
+}
+
+bool isRangeReady()
+{
+  return sensor.isRangeReady();
+}
+
+
+uint16_t getRange()
+{
+  
+  return sensor.readRange();
+}
+
--- a/dev/laser.h
+++ b/dev/laser.h
@ -0,0 +1,10 @@
+
+#pragma once
+#include <cstdint>
+
+bool  Laser_Init(void);          
+short Laser_Read_mm(void);    
+bool Laser_Init_v2(void);
+uint16_t getRange();
+
+bool isRangeReady();
--- a/dev/led.cpp
+++ b/dev/led.cpp
@ -0,0 +1,20 @@
+#include "stm32g4xx.h"
+
+#include "gpio.h"
+
+#include "led.h"
+
+void LED_Init()
+{
+  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
+  GPIO_InitPin(GPIO_PIN_15 | GPIO_PORT_A | GPIO_OUTPUT);
+    GPIO_InitPin(GPIO_PIN_14 | GPIO_PORT_B | GPIO_OUTPUT);
+}
+//------------------------------------------------------------------------------
+
+void LED_Set(bool Set)
+{
+  if (Set) GPIOA->BSRR = GPIO_BSRR_BS_15;
+  else GPIOA->BSRR = GPIO_BSRR_BR_15;
+}
+//------------------------------------------------------------------------------
--- a/dev/led.h
+++ b/dev/led.h
@ -0,0 +1,4 @@
+#pragma once
+
+void LED_Init();
+void LED_Set(bool Set);
--- a/dev/ori.cpp
+++ b/dev/ori.cpp
@ -0,0 +1,281 @@
+#include <math.h>
+
+#include "imu.h"
+#include "bar.h"
+
+#include "med2.h"
+
+#include "ori.h"
+#include "quat.h"
+
+static const float PI = 3.14159265359f;
+static const float TO_DEG = 180.0f/PI;
+static const float TO_RAD = PI/180.0f;
+
+static float FK_PR = 0.01f;
+static float FK_Y = 0.01f;
+
+static float MEGA_BAR;
+static float MEGA_BAR_MIN=0.00001f;
+static float MEGA_BAR_MAX=0.1f;
+static float MEGA_BAR_MUL = 100.0f;
+static float MEGA_BAR_POW = 2.0f;
+static float MEGA_BAR_CNT = 7.0f;
+
+ORI_Data DataORI;
+
+const int BAR_MedCount=50;
+long BAR_MedData[BAR_MedCount];
+unsigned char BAR_MedIndex[BAR_MedCount];
+MED2_Data BAR_Med = {BAR_MedCount, 0, BAR_MedIndex, BAR_MedData, 20};
+
+static float MegaFilterAlt(const long Freq, const float Alt, float& Speed, float& Acc)
+{
+  static float falt=0;
+  falt=falt*(1.0f-MEGA_BAR)+(Alt)*MEGA_BAR;
+  
+  static float alt_speed=0, alt_acc=0;
+  static int count=0;
+  if(count>=MEGA_BAR_CNT)
+  {
+    static float last_b=0, last_s=0;
+    alt_speed=(falt-last_b)*Freq/MEGA_BAR_CNT;
+    alt_acc=(alt_speed-last_s);
+    last_b=falt;
+    last_s=alt_speed;
+    count=0;
+  }
+  else count++;
+  
+  float coef_w, coef_s;
+  
+  coef_w=MEGA_BAR_MIN;
+  float sub=fabsf(Alt-falt);
+  coef_w*=powf(sub*MEGA_BAR_MUL, MEGA_BAR_POW);
+  if(coef_w>MEGA_BAR_MAX) coef_w=MEGA_BAR_MAX;
+  if(coef_w<MEGA_BAR_MIN) coef_w=MEGA_BAR_MIN;
+  
+  
+  MEGA_BAR=coef_w;
+  
+  //static float test_speed=0;
+  //test_speed=(test_speed*15.0f+alt_speed)/16.0f;
+  
+  Speed=MED2_Update(alt_speed*1000, BAR_Med)/1000.0f;
+  
+  Acc=alt_acc;
+  
+  return falt;
+}
+//------------------------------------------------------------------------------
+
+/*const int BAR_MedCount=200;
+long BAR_MedData[BAR_MedCount];
+unsigned char BAR_MedIndex[BAR_MedCount];
+MED2_Data BAR_Med = {BAR_MedCount, 0, BAR_MedIndex, BAR_MedData, 100};
+
+const int BAR_MedCount2=50;
+long BAR_MedData2[BAR_MedCount2];
+unsigned char BAR_MedIndex2[BAR_MedCount2];
+MED2_Data BAR_Med2 = {BAR_MedCount2, 0, BAR_MedIndex2, BAR_MedData2, 10};
+
+static float MegaFilterAlt(const long Freq, const float Alt, float& Speed, float& Acc)
+{
+  static float alt_speed=0, last_b=0;
+  
+  float falt=MED2_Update(Alt*1000.0f, BAR_Med)/1000.0f;
+  
+  static int count=0;
+  if(count>=MEGA_BAR_CNT)
+  {
+    static float last_b=0;
+    alt_speed=(falt-last_b)*Freq/MEGA_BAR_CNT;
+    last_b=falt;
+    count=0;
+  }
+  else count++;
+  
+  Speed=MED2_Update(alt_speed*1000.0f, BAR_Med2)/1000.0f;
+  
+  return falt;
+}*/
+//------------------------------------------------------------------------------
+
+
+
+static float MEGA_ACC=0.01f;
+
+static const long MEGA_Count=60; // 600ms at 100Hz
+static float MEGA_Alt_Buff[MEGA_Count];
+static float MEGA_Spd_Buff[MEGA_Count];
+static long MEGA_Index=0;
+
+static float MegaFilterAcc(struct ORI_Data& Data)
+{
+  float acc;
+    
+  acc=fabsf(Data.Bar.Acc)*100.0f;
+  float filt = MEGA_ACC;
+  if(acc>1) filt/=acc;
+  
+  acc=fabsf(Data.Iner.Z)*100.0f;
+  if(acc>1) filt/=acc;
+  
+  const float g=9.80665f;
+  
+  static float i_speed=0;
+  i_speed+=Data.Iner.Z*g/Data.Freq;
+  
+  
+  MEGA_Alt_Buff[MEGA_Index++]=i_speed;
+  if(MEGA_Index>=MEGA_Count) MEGA_Index=0;
+  static float delay_speed;
+  delay_speed=(delay_speed+MEGA_Alt_Buff[MEGA_Index])/2.0f;
+  
+  float shift=Data.Bar.Speed-delay_speed;
+  
+  for(int a=0; a<MEGA_Count; a++) MEGA_Alt_Buff[a] = MEGA_Alt_Buff[a]*(1.0f-filt) + (MEGA_Alt_Buff[a]+shift)*filt;
+  
+  i_speed = i_speed*(1.0f-filt) + (i_speed+shift)*filt;
+  
+  Data.Speed.Z=i_speed;
+  
+  Data.Pos.Z+=i_speed/Data.Freq;
+  
+  return 0;
+}
+//------------------------------------------------------------------------------
+
+void ORI_Init()
+{ 
+  IMU_Init();
+  MED2_Init(BAR_Med);
+}
+//------------------------------------------------------------------------------
+
+static void GetPos(struct ORI_Data& Data)
+{
+  static long land=Data.Bar.Bar;
+  float alt=0;
+  alt = BAR_GetAltitude(land, Data.Bar.Bar);
+  
+  float alt_speed, alt_acc;
+  
+  float falt=MegaFilterAlt(Data.Freq, alt, alt_speed, alt_acc);
+  
+  Data.Bar.Alt=alt;
+  Data.Bar.Filt=falt;
+  Data.Bar.Speed=alt_speed;
+  Data.Bar.Acc=alt_acc;
+  
+  MegaFilterAcc(Data);
+}
+//------------------------------------------------------------------------------
+
+
+static short NormAcc_X[2]{-7890, 8030}, NormAcc_Y[2]{-8090, 7930}, NormAcc_Z[2]{-7910, 7910};
+
+static short NormMag_X[2]{-160, 160}, NormMag_Y[2]{-155, 140}, NormMag_Z[2]{-150, 140}; // IST8310
+static short GyroShift[3]{-2, 7, 17};
+
+static void NormalizeAcc(short& X, short& Y, short& Z, const short G)
+{
+  short x=(NormAcc_X[1]-NormAcc_X[0])/2;
+  short y=(NormAcc_Y[1]-NormAcc_Y[0])/2;
+  short z=(NormAcc_Z[1]-NormAcc_Z[0])/2;
+  
+  X=(X-(NormAcc_X[0]+x))*G/x;
+  Y=(Y-(NormAcc_Y[0]+y))*G/y;
+  Z=(Z-(NormAcc_Z[0]+z))*G/z;
+}
+
+static void NormalizeMag(short& X, short& Y, short& Z, const short N)
+{
+  short x=(NormMag_X[1]-NormMag_X[0])/2;
+  short y=(NormMag_Y[1]-NormMag_Y[0])/2;
+  short z=(NormMag_Z[1]-NormMag_Z[0])/2;
+  
+  X=(X-(NormMag_X[0]+x))*N/x;
+  Y=(Y-(NormMag_Y[0]+y))*N/y;
+  Z=(Z-(NormMag_Z[0]+z))*N/z;
+}
+
+void ORI_Get(ORI_Data& Data, const unsigned long Freq, IMU_Data& IMU, BAR_Data BAR)
+{
+  Data.Freq=Freq;
+    //------------------------------ imu perevernuta, x z inverse
+  short ax = IMU.Acc.X;
+  short ay = IMU.Acc.Y;
+  short az = IMU.Acc.Z;
+  
+  NormalizeAcc(ax, ay, az, 8000);
+  
+  Data.Acc.X = ax;
+  Data.Acc.Y = ay;
+  Data.Acc.Z = az;
+
+  short gx = (IMU.Gyr.X - GyroShift[0]);
+  short gy = IMU.Gyr.Y - GyroShift[1];
+  short gz = (IMU.Gyr.Z - GyroShift[2]);
+  
+  Data.Gyr.X = gx;
+  Data.Gyr.Y = gy;
+  Data.Gyr.Z = gz;
+  
+  short mx = IMU.Mag.X;
+  short my = IMU.Mag.Y;
+  short mz = IMU.Mag.Z;
+  
+  NormalizeMag(mx, my, mz, 1000);
+  
+  Data.Mag.X = mx;
+  Data.Mag.Y = my;
+  Data.Mag.Z = mz;
+  
+  static float bar_zero=0;
+  static int zero_count=0;
+  
+  if(zero_count<500)
+  {
+    bar_zero=BAR.Pressure;
+    zero_count++;
+  }
+  
+  Data.Bar.Bar=BAR.Pressure;
+  //Data.Bar.Alt=(bar_zero-BAR.Pressure)*9.0f;
+  
+  Vec3 acc{((float)ax)/8000, ((float)ay)/8000, ((float)az)/8000};
+  Vec3 gyr{(float)gx, (float)gy, (float)gz};
+  Vec3 mag{(float)mx, (float)my, -(float)mz};
+  
+  
+
+
+  ORI o = WorkAccGyroMag(acc, gyr, mag, 90, 0.01f);
+  
+  static int test_pitch, test_roll;
+  
+  Data.Pitch=o.Pitch;
+  Data.Roll=o.Roll;
+  Data.Yaw=o.Yaw;
+  
+  Data.SinX=o.sinX;
+  Data.SinY=o.sinY;
+  Data.CosZ=o.cosZ;
+  
+  Data.Iner.X=o.IneX;
+  Data.Iner.Y=o.IneY;
+  Data.Iner.Z=o.IneZ-1;
+  
+  //Data.Speed.X+=o.IneX*9.8f/100.0f;
+  //Data.Speed.Y+=o.IneY*9.8f/100.0f;
+  //Data.Speed.Z+=(o.IneZ-1)*9.8f/100.0f;
+  
+  Data.Temp.Acc=IMU.Temp;
+  Data.Temp.Bar=BAR.Temp;
+  
+  //GetAngle(Data, Freq);
+  //GetCompass(Data);
+  //GetIner(Data);
+  GetPos(Data);
+}
--- a/dev/ori.h
+++ b/dev/ori.h
@ -0,0 +1,29 @@
+#pragma once
+
+struct ORI_Data
+{
+  unsigned long Freq;
+  //---
+  struct { short Acc; short Bar; } Temp;
+  //---
+  struct { short X, Y, Z; } Acc;
+  struct { short X, Y, Z; } Gyr;
+  struct { short X, Y, Z; } Mag;
+  //---
+  struct { float Bar, Alt, Filt, Speed, Acc; } Bar;
+  //---
+  bool Upside = false;
+  float Pitch, Roll, Yaw;
+  float SinX, SinY, CosZ;
+  //---
+  struct { float X, Y, Z; } Iner;
+  struct { float X, Y, Z; } Speed;
+  struct { float X, Y, Z; } Pos;
+};
+
+extern ORI_Data DataORI;
+
+void ORI_Init();
+void ORI_Get(ORI_Data& Data, const unsigned long Freq, struct IMU_Data& IMU, struct BAR_Data BAR);
+
+void ORI_TiltCompAcc(float Pitch, float Roll, short Acc[3], ORI_Data& Data);
--- a/dev/sbus.cpp
+++ b/dev/sbus.cpp
@ -0,0 +1,123 @@
+#include "stm32g4xx.h"
+
+#include <string.h>
+
+#include "uart.h"
+#include "tick.h"
+
+#include "sbus.h"
+
+#pragma pack(push,1)
+union SBUS_Struct
+{
+  unsigned char data[25];
+  struct
+  {
+    unsigned start : 8;
+    unsigned ch1 : 11;
+    unsigned ch2 : 11;
+    unsigned ch3 : 11;
+    unsigned ch4 : 11;
+    unsigned ch5 : 11;
+    unsigned ch6 : 11;
+    unsigned ch7 : 11;
+    unsigned ch8 : 11;
+    unsigned ch9 : 11;
+    unsigned ch10 : 11;
+    unsigned ch11 : 11;
+    unsigned ch12 : 11;
+    unsigned ch13 : 11;
+    unsigned ch14 : 11;
+    unsigned ch15 : 11;
+    unsigned ch16 : 11;
+    unsigned flags : 8;
+    unsigned end : 8;
+  } bus;
+};
+#pragma pack(pop)
+
+// IBUS: LEN(1)+CMD(1)+DATA(0..32)+CHSM(2)
+
+#define SBUS_START 0x0F
+
+#define SBUS_CH17 0x01
+#define SBUS_CH18 0x02
+
+#define SBUS_FRAMELOST 0x04
+#define SBUS_FAILSAFE 0x08
+
+#define SBUS_LENGTH 25
+
+void SBUS_Init()
+{
+  UART1_Init(100'000);
+  USART1->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_RXNEIE;
+  USART1->CR2 = USART_CR2_STOP_1 | USART_CR2_RXINV;
+  USART1->CR1 |= USART_CR1_PCE | USART_CR1_PS | USART_CR1_UE;
+}
+
+static const unsigned long Size = SBUS_LENGTH;
+static char Buffer[Size];
+
+static char Length = 0;
+
+static unsigned long Time;
+
+static bool Parse(SBUS_Data& Data, char byte)
+{
+  unsigned long tick=TICK_GetCount();
+  
+  unsigned long wait = tick - Time;
+  if (wait > 4) Length = 0; // Protocol synchronization lost !!!
+
+  Time=tick;
+  
+  if (!Length && (byte != SBUS_START)) return false;
+
+  Buffer[Length++] = byte;
+  
+  if(Length<Size) return false;
+  Length=0;
+  
+  SBUS_Struct* frame=(SBUS_Struct*)Buffer;
+  if(frame->bus.end!=0) return false;
+  
+  Data.X=frame->bus.ch1;
+  Data.Y=frame->bus.ch2;
+  Data.Z=frame->bus.ch3;
+  Data.W=frame->bus.ch4;
+  
+  Data.SWA=frame->bus.ch5;
+  Data.SWB=frame->bus.ch6;
+  Data.SWC=frame->bus.ch7;
+  Data.SWD=frame->bus.ch8;
+  
+  Data.VRA=frame->bus.ch9;
+  Data.VRB=frame->bus.ch10;
+  
+  Data.OTHER[0]=frame->bus.ch11;
+  Data.OTHER[1]=frame->bus.ch12;
+  Data.OTHER[2]=frame->bus.ch13;
+  Data.OTHER[3]=frame->bus.ch14;
+  Data.OTHER[4]=frame->bus.ch15;
+  Data.OTHER[5]=frame->bus.ch16;
+  
+  Data.OTHER[6]=(bool)(frame->bus.flags & SBUS_CH17);
+  Data.OTHER[7]=(bool)(frame->bus.flags & SBUS_CH18);
+  
+  Data.FailSafe=frame->bus.flags & SBUS_FAILSAFE;
+  Data.FrameLost=frame->bus.flags & SBUS_FRAMELOST;
+  
+  return true;
+}
+
+bool SBUS_Update(SBUS_Data& Data, bool& Off)
+{
+  char buf[Size];
+  unsigned long size = UART1_Recv(buf, Size);
+
+  bool done = false;
+  for (long a = 0; a < size; a++) done = Parse(Data, buf[a]);
+  Off=Data.FailSafe;
+  return done;
+}
--- a/dev/sbus.h
+++ b/dev/sbus.h
@ -0,0 +1,25 @@
+#pragma once
+
+#define JOY_MIN 174
+
+#define JOY_MID 996
+#define JOY_MID_PITCH 983
+#define JOY_MID_ROLL 996
+#define JOY_MID_YAW 996
+#define JOY_MAX 1811
+
+#define JOY_VAL 1.567f
+
+struct SBUS_Data // Radiomaster
+{
+  short Z, W, X, Y;
+  short SWA, SWB, SWC, SWD;
+  short VRA, VRB;
+  short OTHER[8];
+  
+  bool FrameLost;
+  bool FailSafe;
+};
+
+void SBUS_Init();
+bool SBUS_Update(SBUS_Data& Data, bool& Off);
--- a/dev/tele.cpp
+++ b/dev/tele.cpp
@ -0,0 +1,193 @@
+#include <string.h>
+
+#include "gpio.h"
+
+#include "uart.h"
+#include "tick.h"
+
+#include "tele.h"
+
+enum class RecvModeEnum : unsigned char { Begin, Head, Titles, Data, Auto, SetPID, GetPID, Done };
+
+struct RecvHead
+{
+  RecvHead(){};
+  RecvHead(RecvModeEnum mode, unsigned char size, unsigned char crc)
+  {
+    Mode = mode;
+    DataSize = size;
+    DataCRC8 = crc;
+    CRC8 = (unsigned char)((unsigned char)Mode ^ DataSize ^ DataCRC8);
+  }
+
+  RecvModeEnum Mode;
+  unsigned char DataSize;
+  unsigned char DataCRC8;
+
+  unsigned char CRC8;
+  
+  bool Check()
+  {
+    return CRC8 == (unsigned char)((unsigned char)Mode ^ DataSize ^ DataCRC8);
+  }
+};
+
+unsigned char GetCRC8(const void* arr, int size)
+{
+  unsigned char crc = 0;
+  for (int a = 0; a < size; a++) crc ^= ((unsigned char*)arr)[a];
+  return crc;
+}
+
+static RecvHead Tele_Mode(RecvModeEnum::Begin,0,0);
+
+void TELE_Init()
+{
+  UART3_Init(57600);
+}
+
+static unsigned long TELE_LastTime=0;
+static unsigned long TELE_WaitTimer=0;
+
+static unsigned long TELE_Count=0;
+
+static bool DataAuto=true;
+
+void TELE_Update(const void* info, unsigned long size, unsigned long update)
+{
+  
+  switch(Tele_Mode.Mode)
+  {
+    case RecvModeEnum::Begin:
+    {
+      unsigned char begin;
+      int len=UART3_Recv(&begin, 1);
+      TELE_WaitTimer=TICK_GetCount();
+      if(len && begin==0xAA) Tele_Mode.Mode=RecvModeEnum::Head;
+      break;
+    }
+    
+    case RecvModeEnum::Head:
+    {
+      int len=UART3_Recv(0, 0);
+      if(len<sizeof(RecvHead)) break;
+      
+      RecvHead head;
+      UART3_Recv(&head, sizeof(RecvHead));
+      
+      if(!head.Check()) Tele_Mode.Mode=RecvModeEnum::Begin;
+      else Tele_Mode=head;
+      
+      break;
+    }
+    
+    case RecvModeEnum::Titles:
+    {
+      const char* titles="Acc X|AccY|Acc Z|Acc S|Gyr X|Gyr Y|Gyr Z|Pitch|Roll|Yaw";
+      int len=strlen(titles);
+      
+      unsigned char h=0xAA;
+      UART3_Send(&h, 1);
+      
+      RecvHead head(RecvModeEnum::Titles, len, GetCRC8(titles, len));
+      UART3_Send(&head, sizeof(head));
+      UART3_Send(titles, len);
+      
+      Tele_Mode.Mode=RecvModeEnum::Begin;
+      
+      break;
+    }
+    
+    /*case RecvModeEnum::GetPID:
+    {
+      float send[20]={PID_X.Min, PID_X.Max, 
+      
+      PID_X.P.Min, PID_X.P.Max, PID_X.P.C,
+      PID_X.I.Min, PID_X.I.Max, PID_X.I.C,
+      PID_X.D.Min, PID_X.D.Max, PID_X.D.C,
+      
+      PID_Z.P.Min, PID_Z.P.Max, PID_Z.P.C,
+      PID_Z.I.Min, PID_Z.I.Max, PID_Z.I.C,
+      PID_Z.D.Min, PID_Z.D.Max, PID_Z.D.C
+      };
+      
+      unsigned char h=0xAA;
+      UART3_Send(&h, 1);
+      
+      RecvHead head(RecvModeEnum::GetPID, sizeof(send), GetCRC8(send, sizeof(send)));
+      UART3_Send(&head, sizeof(head));
+      UART3_Send(send, sizeof(send));
+      
+      Tele_Mode.Mode=RecvModeEnum::Begin;
+      
+      break;
+    }*/
+    
+    case RecvModeEnum::Data:
+    {
+      unsigned char h=0xAA;
+      UART3_Send(&h, 1);
+      
+      RecvHead head(RecvModeEnum::Data, size, GetCRC8(info, size));
+      UART3_Send(&head, sizeof(head));
+      UART3_Send(info, size);
+      
+      Tele_Mode.Mode=RecvModeEnum::Begin;
+      
+      break;
+    }
+    
+    /*case RecvModeEnum::SetPID:
+    {
+      float recv[20];
+      
+      unsigned long len=UART3_Recv(0, 0);
+      if(len<sizeof(recv)) break;
+      
+      UART3_Recv(recv, sizeof(recv));
+      
+      unsigned char crc=GetCRC8(recv, sizeof(recv));
+      if(Tele_Mode.DataCRC8!=crc) break;
+      
+      PID_X.Min=recv[0]; PID_X.Max=recv[1];
+      
+      PID_X.P.Min=recv[2]; PID_X.P.Max=recv[3]; PID_X.P.C=recv[4];
+      PID_X.I.Min=recv[5]; PID_X.I.Max=recv[6]; PID_X.I.C=recv[7];
+      PID_X.D.Min=recv[8]; PID_X.D.Max=recv[9]; PID_X.D.C=recv[10];
+      
+      PID_Z.P.Min=recv[11]; PID_Z.P.Max=recv[12]; PID_Z.P.C=recv[13];
+      PID_Z.I.Min=recv[14]; PID_Z.I.Max=recv[15]; PID_Z.I.C=recv[16];
+      PID_Z.D.Min=recv[17]; PID_Z.D.Max=recv[18]; PID_Z.D.C=recv[19];
+      
+      unsigned char h=0xAA;
+      UART3_Send(&h, 1);
+      
+      RecvHead head(RecvModeEnum::Done, 0, 0);
+      UART3_Send(&head, sizeof(head));
+      
+      Tele_Mode.Mode=RecvModeEnum::Begin;
+      
+      break;
+    }*/
+    
+    default:
+    {
+      Tele_Mode.Mode=RecvModeEnum::Begin;
+      break;
+    }
+  }
+  
+  if(TICK_GetCount()-TELE_WaitTimer>100) Tele_Mode.Mode=RecvModeEnum::Begin;
+  
+  if(DataAuto && (TICK_GetCount()-TELE_LastTime>update))
+  {
+    TELE_LastTime=TICK_GetCount();
+    
+    unsigned char h=0xAA;
+    UART3_Send(&h, 1);
+    
+    RecvHead head(RecvModeEnum::Data, size, GetCRC8(info, size));
+    UART3_Send(&head, sizeof(head));
+    UART3_Send(info, size);
+  }
+}
--- a/dev/tele.h
+++ b/dev/tele.h
@ -0,0 +1,4 @@
+#pragma once
+
+void TELE_Init();
+void TELE_Update(const void* info, unsigned long size, unsigned long update);