add firmware

Fly.eww

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<workspace>
+    <project>
+        <path>$WS_DIR$\Fly.ewp</path>
+    </project>
+    <batchBuild />
+</workspace>

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;
+  
+}
+//------------------------------------------------------------------------------

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);

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)
+//{
+//  
+//}

dev/com.h

@@ -0,0 +1,4 @@
+#pragma once
+
+void COM_Init();
+void COM_Update();

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();
+}
+//------------------------------------------------------------------------------

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);

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;
+}

dev/flow.h

@@ -0,0 +1,4 @@
+#pragma once
+
+bool FLOW_Init();
+bool FLOW_GetMotion(short* dX, short* dY, unsigned char* qual);

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;
+}
+//------------------------------------------------------------------------------

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);

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);
+  
+}
+//------------------------------------------------------------------------------

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);

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();
+}
+

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();

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;
+}
+//------------------------------------------------------------------------------

dev/led.h

@@ -0,0 +1,4 @@
+#pragma once
+
+void LED_Init();
+void LED_Set(bool Set);

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);
+}

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);

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;
+}

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);

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);
+  }
+}

dev/tele.h

@@ -0,0 +1,4 @@
+#pragma once
+
+void TELE_Init();
+void TELE_Update(const void* info, unsigned long size, unsigned long update);

drv/gpio.cpp

@@ -0,0 +1,37 @@
+#include "stm32g4xx.h"
+
+#include "gpio.h"
+
+void GPIO_InitPin(unsigned long I_Pin)
+{
+  unsigned long port = (I_Pin & 0x000000F0UL) >> 4;
+  GPIO_TypeDef* gpio = (GPIO_TypeDef*)(((unsigned char*)GPIOA) + (port * 0x0400));
+  unsigned long rcc = 1UL << port;
+  unsigned long pin = I_Pin & 0x0000000FUL;
+  unsigned long af = (I_Pin & 0x0F000000UL) >> 24;
+  unsigned long pupd = (I_Pin & 0x00F00000UL) >> 20;
+  unsigned long ospeed = (I_Pin & 0x000F0000UL) >> 16;
+  unsigned long mode = (I_Pin & 0x0000F000UL) >> 12;
+  unsigned long otype = (I_Pin & 0x00000100UL) >> 8;
+  unsigned long set = (I_Pin & 0x00000200UL) >> 9;
+  //---
+  if (!(RCC->AHB2ENR & rcc)) RCC->AHB2ENR |= rcc;
+  //---
+  gpio->AFR[pin >> 3] &= ~(0x0000000FUL << ((pin & 0x07) * 4));
+  gpio->AFR[pin >> 3] |= af << ((pin & 0x07) * 4);
+  //---
+  gpio->OSPEEDR &= ~(0x00000003UL << (pin * 2));
+  gpio->OSPEEDR |= ospeed << (pin * 2);
+  //---
+  gpio->OTYPER &= ~(0x00000001UL << pin);
+  gpio->OTYPER |= otype << pin;
+  //---
+  gpio->PUPDR &= ~(0x00000003UL << (pin * 2));
+  gpio->PUPDR |= pupd << (pin * 2);
+  //---
+  gpio->BSRR = 1 << (set ? pin : pin+16);
+  //---
+  gpio->MODER &= ~(0x00000003UL << (pin * 2));
+  gpio->MODER |= mode << (pin * 2);
+}
+//------------------------------------------------------------------------------

drv/gpio.h

@@ -0,0 +1,69 @@
+#pragma once
+
+#define GPIO_PIN_0  0x00000000UL
+#define GPIO_PIN_1  0x00000001UL
+#define GPIO_PIN_2  0x00000002UL
+#define GPIO_PIN_3  0x00000003UL
+#define GPIO_PIN_4  0x00000004UL
+#define GPIO_PIN_5  0x00000005UL
+#define GPIO_PIN_6  0x00000006UL
+#define GPIO_PIN_7  0x00000007UL
+#define GPIO_PIN_8  0x00000008UL
+#define GPIO_PIN_9  0x00000009UL
+#define GPIO_PIN_10 0x0000000AUL
+#define GPIO_PIN_11 0x0000000BUL
+#define GPIO_PIN_12 0x0000000CUL
+#define GPIO_PIN_13 0x0000000DUL
+#define GPIO_PIN_14 0x0000000EUL
+#define GPIO_PIN_15 0x0000000FUL
+
+#define GPIO_PORT_A 0x00000000UL
+#define GPIO_PORT_B 0x00000010UL
+#define GPIO_PORT_C 0x00000020UL
+#define GPIO_PORT_D 0x00000030UL
+#define GPIO_PORT_E 0x00000040UL
+#define GPIO_PORT_F 0x00000070UL
+#define GPIO_PORT_G 0x00000080UL
+#define GPIO_PORT_H 0x00000090UL
+#define GPIO_PORT_I 0x000000A0UL
+#define GPIO_PORT_J 0x000000B0UL
+#define GPIO_PORT_K 0x000000C0UL
+
+#define GPIO_PUSHPULL  0x00000000UL
+#define GPIO_OPENDRAIN 0x00000100UL
+
+#define GPIO_RESET 0x00000000UL
+#define GPIO_SET   0x00000200UL
+
+#define GPIO_INPUT  0x00000000UL
+#define GPIO_OUTPUT 0x00001000UL
+#define GPIO_ALTER  0x00002000UL
+#define GPIO_ANALOG 0x00003000UL
+
+#define GPIO_OSPEED_LOW    0x00000000UL
+#define GPIO_OSPEED_MEDIUM 0x00010000UL
+#define GPIO_OSPEED_FAST   0x00020000UL
+#define GPIO_OSPEED_HIGH   0x00030000UL
+
+#define GPIO_NOPUPD   0x00000000UL
+#define GPIO_PULLUP   0x00100000UL
+#define GPIO_PULLDOWN 0x00200000UL
+
+#define GPIO_AF0  0x00000000UL
+#define GPIO_AF1  0x01000000UL
+#define GPIO_AF2  0x02000000UL
+#define GPIO_AF3  0x03000000UL
+#define GPIO_AF4  0x04000000UL
+#define GPIO_AF5  0x05000000UL
+#define GPIO_AF6  0x06000000UL
+#define GPIO_AF7  0x07000000UL
+#define GPIO_AF8  0x08000000UL
+#define GPIO_AF9  0x09000000UL
+#define GPIO_AF10 0x0A000000UL
+#define GPIO_AF11 0x0B000000UL
+#define GPIO_AF12 0x0C000000UL
+#define GPIO_AF13 0x0D000000UL
+#define GPIO_AF14 0x0E000000UL
+#define GPIO_AF15 0x0F000000UL
+
+void GPIO_InitPin(unsigned long I_Pin);

drv/i2c.cpp

@@ -0,0 +1,201 @@
+#include "stm32g4xx.h"
+
+#include <string.h>
+
+#include "gpio.h"
+
+#include "i2c.h"
+
+static void Init(I2C_TypeDef* I2C)
+{
+  I2C->TIMINGR = 0x00303D5BUL; // 100kHz
+  
+  //I2C->TIMINGR = 	0x30108DFF;
+  
+  I2C->CR1 = I2C_CR1_PE;
+
+  while(I2C->ISR & I2C_ISR_BUSY) { }
+}
+//------------------------------------------------------------------------------
+
+void I2C2_Init()
+{
+  if (RCC->APB1ENR1 & RCC_APB1ENR1_I2C2EN) return;
+
+  RCC->APB1ENR1 |= RCC_APB1ENR1_I2C2EN;
+
+  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
+
+  GPIO_InitPin(GPIO_PIN_8 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);
+  GPIO_InitPin(GPIO_PIN_9 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);
+
+  Init(I2C2);
+}
+//------------------------------------------------------------------------------
+
+void I2C1_Init()
+{
+  if (RCC->APB1ENR1 & RCC_APB1ENR1_I2C1EN) return;
+
+  RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
+
+  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
+
+  GPIO_InitPin(GPIO_PIN_8 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);
+  GPIO_InitPin(GPIO_PIN_9 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);
+
+  Init(I2C1);
+}
+//------------------------------------------------------------------------------
+
+static void Stop(I2C_TypeDef* I2C)
+{
+  I2C->CR2 |= I2C_CR2_STOP;
+  while (!(I2C->ISR & I2C_ISR_STOPF)) { asm volatile("NOP"); }
+  I2C->ICR = I2C_ICR_STOPCF;
+}
+//------------------------------------------------------------------------------
+
+static void Read(I2C_TypeDef* I2C, unsigned char Address, unsigned char* Data, unsigned char Size)
+{
+  I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_NBYTES_Msk);
+  I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | I2C_CR2_RD_WRN | (((unsigned long)Size)<<I2C_CR2_NBYTES_Pos);
+  I2C->CR2 |= I2C_CR2_START;
+  
+  while (Size--)
+  {
+    while (!(I2C->ISR & I2C_ISR_RXNE)) { }
+    *Data++ = I2C->RXDR;
+  }
+
+
+    
+    
+}
+//------------------------------------------------------------------------------
+
+static void Write(I2C_TypeDef* I2C, unsigned char Address, const unsigned char* Data, unsigned char Size)
+{
+  I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
+  I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | (((unsigned long)Size)<<I2C_CR2_NBYTES_Pos);
+  I2C->CR2 |= I2C_CR2_START;
+
+  while(I2C->CR2 & I2C_CR2_START) { asm volatile("NOP"); }
+  
+  while (Size--)
+  {
+    while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
+    I2C->TXDR = *Data++;
+  }
+  
+  while(!(I2C->ISR & I2C_ISR_TC))  { asm volatile("NOP"); }//???
+
+
+
+   
+
+}
+//------------------------------------------------------------------------------
+
+static void Write2(I2C_TypeDef* I2C, unsigned char Address, const unsigned char* Data1, unsigned char Size1, const unsigned char* Data2, unsigned char Size2)
+{
+  I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
+  I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | (((unsigned long)Size1+Size2)<<I2C_CR2_NBYTES_Pos);
+  I2C->CR2 |= I2C_CR2_START;
+  
+  while (Size1--)       
+  {
+    while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
+    I2C->TXDR = *Data1++;
+  }
+  
+  while (Size2--)
+  {
+    while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
+    I2C->TXDR = *Data2++;
+  }
+  
+  while(!(I2C->ISR & I2C_ISR_TC))  { asm volatile("NOP"); }
+
+           
+}
+//------------------------------------------------------------------------------
+
+void I2C2_Write(unsigned char Address, unsigned char Data)
+{
+  Write(I2C2, Address, &Data, 1);
+}
+//------------------------------------------------------------------------------
+
+void I2C2_Write(unsigned char Address, const void* Data, unsigned char Size)
+{
+  Write(I2C2, Address, (unsigned char*)Data, Size);
+}
+//------------------------------------------------------------------------------
+
+void I2C2_Read(unsigned char Address, void* Data, unsigned char Size)
+{
+  Read(I2C2, Address, (unsigned char*)Data, Size);
+}
+//------------------------------------------------------------------------------
+
+void I2C2_Stop()
+{
+  Stop(I2C2);
+}
+//------------------------------------------------------------------------------
+
+
+
+void I2C1_Write(unsigned char Address, unsigned char Data)
+{
+  Write(I2C1, Address, &Data, 1);
+}
+//------------------------------------------------------------------------------
+
+void I2C1_Write(unsigned char Address, const void* Data, unsigned char Size)
+{
+  Write(I2C1, Address, (unsigned char*)Data, Size);
+}
+//------------------------------------------------------------------------------
+
+void I2C1_Write2(unsigned char Address, const void* Data1, unsigned char Size1, const void* Data2, unsigned char Size2)
+{
+  Write2(I2C1, Address, (unsigned char*)Data1, Size1, (unsigned char*)Data2, Size2);
+}
+//------------------------------------------------------------------------------
+
+void I2C1_Read(unsigned char Address, void* Data, unsigned char Size)
+{
+  Read(I2C1, Address, (unsigned char*)Data, Size);
+}
+//------------------------------------------------------------------------------
+
+void I2C1_Stop()
+{
+  Stop(I2C1);
+}
+//------------------------------------------------------------------------------
+
+
+
+extern "C" {
+
+int8_t I2C1_WriteBytes(uint8_t addr, const uint8_t *data, uint8_t size)
+{
+    Write(I2C1, addr, (unsigned char*)data, size);
+ 
+    return 0;                 
+}
+
+int8_t I2C1_ReadBytes(uint8_t addr, uint8_t *data, uint8_t size)
+{
+    Read(I2C1, addr,(unsigned char*)data, size);
+
+    return 0;
+}
+
+} // extern "C"
+
+
+

drv/i2c.h

@@ -0,0 +1,27 @@
+#pragma once
+
+struct I2C_Device
+{
+  unsigned char* Buffer;
+  unsigned char Size;
+  void (*CallbackProc)(unsigned char Address, const unsigned char* Data, unsigned char Size);
+  
+  unsigned char Address;
+  unsigned char Write;
+  unsigned char Read;
+  
+  I2C_Device* Next;
+};
+
+void I2C2_Init();
+void I2C2_Write(unsigned char Address, unsigned char Data);
+void I2C2_Write(unsigned char Address, const void* Data, unsigned char Size);
+void I2C2_Read(unsigned char Address, void* Data, unsigned char Size);
+void I2C2_Stop();
+
+void I2C1_Init();
+void I2C1_Write(unsigned char Address, unsigned char Data);
+void I2C1_Write(unsigned char Address, const void* Data, unsigned char Size);
+void I2C1_Write2(unsigned char Address, const void* Data1, unsigned char Size1, const void* Data2, unsigned char Size2);
+void I2C1_Read(unsigned char Address, void* Data, unsigned char Size);
+void I2C1_Stop();

drv/iwdg.cpp

@@ -0,0 +1,18 @@
+#include "stm32g4xx.h"
+
+#include "iwdg.h"
+
+void InitIWDG(unsigned short Reload)
+{
+  IWDG->KR=0x0000CCCC;
+  IWDG->KR=0x00005555;
+  IWDG->PR=0;
+  IWDG->RLR=Reload;
+  while(IWDG->SR) { asm volatile("NOP"); }
+  IWDG->KR=0x0000AAAA;
+}
+
+void PingIWDG()
+{
+  IWDG->KR=0x0000AAAA;
+}

drv/iwdg.h

@@ -0,0 +1,4 @@
+#pragma once
+
+void InitIWDG(unsigned short Reload);
+void PingIWDG();

drv/pwm.cpp

@@ -0,0 +1,85 @@
+#include "stm32g4xx.h"
+
+#include "gpio.h"
+
+#include "pwm.h"
+
+bool PWM_Enable=false;
+short PWM_work_min = 1050;//???
+
+inline static unsigned short Mid(unsigned short Val, unsigned short Min, unsigned short Max)
+{
+  if(Val<Min) return Min;
+  if(Val>Max) return Max;
+  return Val;
+}
+
+
+void PWM_Init(unsigned long Freq)
+{
+  GPIO_InitPin(GPIO_PIN_0 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF1);
+  GPIO_InitPin(GPIO_PIN_1 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF1);
+  GPIO_InitPin(GPIO_PIN_2 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF1);
+  GPIO_InitPin(GPIO_PIN_3 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF1);
+  
+  RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
+  TIM2->CR2 = 0;
+  
+
+  
+  TIM2->ARR = 1000 - 1; //The comparison range is 100     
+  TIM2->PSC = (16'000'000/1000/16000)-1; // The divider is set to 16000Hz
+  
+  TIM2->CCMR1 = TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1PE; //PWM mode 1 for CH1
+  TIM2->CCMR1|= TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2PE; // PWM mode 1 for CH2
+  TIM2->CCMR2 = TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3PE; // PWM mode 1 for  CH3
+  TIM2->CCMR2|= TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4PE; // PWM mode 1 for CH4
+  
+  TIM2->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E |TIM_CCER_CC4E; // CH1 and CH2 outputs are active
+  TIM2->BDTR = TIM_BDTR_MOE; // Allow active outputs to work
+  TIM2->CR1 = TIM_CR1_CEN; // Timer is running
+  for(int a=0; a<100000; a++) { asm volatile("NOP");}
+  
+}
+//------------------------------------------------------------------------------
+
+static void Stop()
+{
+  PWM_Enable=false;
+
+  TIM2->CCER=0;
+
+}
+//------------------------------------------------------------------------------
+
+void PWM_SetQuad(short Pow[4], unsigned short Min, unsigned short Max)
+{
+  if(!Pow) { Stop(); return; }
+  TIM2->CCR1 = Mid(Pow[0], Min, Max); //M1
+  TIM2->CCR2 = Mid(Pow[1], Min, Max); //M2
+  TIM2->CCR3 = Mid(Pow[2], Min, Max); //M3
+  TIM2->CCR4 = Mid(Pow[3], Min, Max); //M4
+  
+  
+
+  bool en=false;
+  for(int a=0; a<4; a++) if(Pow[a]>PWM_work_min) en=true;
+  PWM_Enable=en;
+}
+//------------------------------------------------------------------------------
+
+
+
+void PWM_SetAll(unsigned short Pow)
+{
+
+  TIM2->CCR1 = Pow; 
+  TIM2->CCR2 = Pow; 
+  TIM2->CCR3 = Pow; 
+  TIM2->CCR4 = Pow; 
+  
+  bool en=false;
+  if(Pow>PWM_work_min) en=true;
+  PWM_Enable=en;
+}
+//------------------------------------------------------------------------------

drv/pwm.h

@@ -0,0 +1,7 @@
+#pragma once
+
+extern bool PWM_Enable;
+
+void PWM_Init(unsigned long Freq);
+void PWM_SetQuad(short M[4], unsigned short Min, unsigned short Max);
+void PWM_SetAll(unsigned short Pow);

drv/spi.cpp

@@ -0,0 +1,138 @@
+#include "stm32g4xx.h"
+
+#include "gpio.h"
+
+#include "spi.h"
+
+static void Init(SPI_TypeDef* SPI)
+{
+  SPI->CR1 = SPI_CR1_CPHA | SPI_CR1_CPOL | SPI_CR1_SSI | SPI_CR1_SSM | (SPI_CR1_BR_0 | SPI_CR1_BR_1) | SPI_CR1_MSTR;
+  SPI->CR2 = SPI_CR2_DS_0 | SPI_CR2_DS_1 | SPI_CR2_DS_2 | SPI_CR2_FRXTH;
+  SPI->CR1 |= SPI_CR1_SPE;
+}
+//------------------------------------------------------------------------------
+
+void SPI1_Init()
+{
+  if(RCC->APB2ENR & RCC_APB2ENR_SPI1EN) return;
+
+  RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
+
+  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
+  GPIO_InitPin(GPIO_PIN_5 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF5 | GPIO_OSPEED_HIGH); // SCK
+  GPIO_InitPin(GPIO_PIN_6 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF5 | GPIO_OSPEED_HIGH); // MISO
+  
+  GPIO_InitPin(GPIO_PIN_7 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF5 | GPIO_OSPEED_HIGH); // MOSI
+  
+  
+  
+  GPIO_InitPin(GPIO_PIN_4  | GPIO_PORT_A | GPIO_OUTPUT | GPIO_OSPEED_HIGH | GPIO_SET); // NCS -> PMW3901
+  
+  
+  
+
+  Init(SPI1);
+}
+//------------------------------------------------------------------------------
+
+static void Trans(SPI_TypeDef* SPI, const void* WriteData, void* ReadData, unsigned long Size)
+{
+  unsigned char* dr=(unsigned char*)ReadData;
+  unsigned char* dw=(unsigned char*)WriteData;
+  
+  volatile char& DR = *((volatile char*)&(SPI->DR));
+
+  while (Size--)
+  {
+    while (!(SPI->SR & SPI_SR_TXE)) { asm volatile("NOP"); }
+    if (dw) DR = *(dw++);
+    else DR = 0x00;
+
+    while (!(SPI->SR & SPI_SR_RXNE)) { asm volatile("NOP"); }
+    if (dr) *(dr++) = DR;
+    else volatile char t = DR;
+  }
+
+  while (SPI->SR & SPI_SR_BSY) { asm volatile("NOP"); }
+}
+//------------------------------------------------------------------------------
+
+static inline void CS_IMU(bool En)
+{
+  if(En) GPIOB->BSRR = GPIO_BSRR_BR_12;
+  else GPIOB->BSRR = GPIO_BSRR_BS_12;
+  
+  for (unsigned long a = 0; a < 21; a++) { asm volatile("NOP"); }
+}
+//------------------------------------------------------------------------------
+
+void SPI1_TransferCons(const void* WriteData, unsigned long WriteSize, void* ReadData, unsigned long ReadSize)
+{
+  CS_IMU(true);
+
+  Trans(SPI1, WriteData, 0, WriteSize);
+  Trans(SPI1, 0, ReadData, ReadSize);
+
+  CS_IMU(false);
+}
+//------------------------------------------------------------------------------
+
+void SPI1_TransferParallel(const void* WriteData, void* ReadData, unsigned long Size)
+{
+  CS_IMU(true);
+
+  Trans(SPI1, WriteData, ReadData, Size);
+
+  CS_IMU(false);
+}
+//------------------------------------------------------------------------------
+
+//------------------------------------------------------------------------------
+
+void SPI2_Init()
+{
+
+  if ( RCC->APB1ENR1&RCC_APB1ENR1_SPI2EN) return;
+
+  RCC->APB1ENR1 |= RCC_APB1ENR1_SPI2EN;
+
+  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
+  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
+  
+ GPIO_InitPin(GPIO_PIN_10 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF5 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP); // MISO
+  GPIO_InitPin(GPIO_PIN_11 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF5 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP); // MOSI
+  
+
+  GPIO_InitPin(GPIO_PIN_13 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF5 | GPIO_OSPEED_HIGH| GPIO_OPENDRAIN | GPIO_PULLUP); // SCK
+  GPIO_InitPin(GPIO_PIN_12  | GPIO_PORT_B | GPIO_OUTPUT | GPIO_OSPEED_HIGH | GPIO_SET| GPIO_OPENDRAIN | GPIO_PULLUP); // NSS -> PMW3901
+  
+
+  
+  
+
+  Init(SPI2);
+}
+//------------------------------------------------------------------------------
+
+//------------------------------------------------------------------------------
+
+void SPI2_TransferCons(const void* WriteData, unsigned long WriteSize, void* ReadData, unsigned long ReadSize)
+{
+  CS_IMU(true);
+
+  Trans(SPI2, WriteData, 0, WriteSize);
+  Trans(SPI2, 0, ReadData, ReadSize);
+
+  CS_IMU(false);
+}
+//------------------------------------------------------------------------------
+
+void SPI2_TransferParallel(const void* WriteData, void* ReadData, unsigned long Size)
+{
+  CS_IMU(true);
+
+  Trans(SPI2, WriteData, ReadData, Size);
+
+  CS_IMU(false);
+}
+//-----------------------------------------------------------------------------

drv/spi.h

@@ -0,0 +1,12 @@
+#pragma once
+
+void SPI1_Init();
+
+void SPI1_TransferCons(const void* WriteData, unsigned long WriteSize, void* ReadData, unsigned long ReadSize);
+void SPI1_TransferParallel(const void* WriteData, void* ReadData, unsigned long Size);
+
+
+void SPI2_Init();
+
+void SPI2_TransferCons(const void* WriteData, unsigned long WriteSize, void* ReadData, unsigned long ReadSize);
+void SPI2_TransferParallel(const void* WriteData, void* ReadData, unsigned long Size);

drv/tick.cpp

@@ -0,0 +1,36 @@
+#include "stm32g4xx.h"
+
+#include "tick.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;
+}
+//------------------------------------------------------------------------------
+
+
+void Tick_Delay(unsigned long ms)
+{
+    const unsigned long start = Tick;
+    while ((Tick - start) < ms) {
+        __NOP();                     
+    }
+}

drv/tick.h

@@ -0,0 +1,5 @@
+#pragma once
+
+void TICK_Init();
+unsigned long TICK_GetCount();
+void  Tick_Delay(unsigned long ms);

drv/tim.cpp

@@ -0,0 +1,34 @@
+#include "stm32g4xx.h"
+
+#include "tim.h"
+
+static void (*TIM7_Proc)() = 0;
+
+extern "C" void TIM7_IRQHandler()
+{
+  TIM7->SR = 0;
+  TIM7_Proc();
+}
+//------------------------------------------------------------------------------
+
+void TIM7_Init(long Priority)
+{
+  RCC->APB1ENR1 |= RCC_APB1ENR1_TIM7EN;
+
+  TIM7->CR1 = 0;
+  TIM7->ARR = 1000 - 1;
+  TIM7->DIER = TIM_DIER_UIE;
+
+  NVIC_SetPriority(TIM7_IRQn, Priority);
+  NVIC_EnableIRQ(TIM7_IRQn);
+}
+//------------------------------------------------------------------------------
+
+void TIM7_Update(unsigned long Freq, void (*Proc)())
+{
+  TIM7->CR1 = 0;
+  TIM7->PSC = (SystemCoreClock / 1000 / Freq) - 1;
+  TIM7_Proc = Proc;
+  TIM7->CR1 = TIM_CR1_CEN;
+}
+//------------------------------------------------------------------------------

drv/tim.h

@@ -0,0 +1,4 @@
+#pragma once
+
+void TIM7_Init(long Priority);
+void TIM7_Update(unsigned long Freq, void (*UpdateProc)());

drv/uart.cpp

@@ -0,0 +1,237 @@
+#include "stm32g4xx.h"
+
+#include <string.h>
+
+#include "gpio.h"
+
+#include "uart.h"
+
+static const unsigned long UART_BuferSize=256;
+
+struct UART_Data
+{
+  USART_TypeDef* UART;
+
+  struct
+  {
+    unsigned long Size;
+    unsigned char* Buffer;
+    unsigned short Head;
+    unsigned short Tail;
+    unsigned short Push;
+    unsigned short Pull;
+  } Recv, Send;
+};
+
+static unsigned char UART3_BufferRecv[UART_BuferSize], UART3_BufferSend[UART_BuferSize];
+static UART_Data UART3_Data{ USART3, {sizeof(UART3_BufferRecv), UART3_BufferRecv, 0, 0, 0, 0}, {sizeof(UART3_BufferSend), UART3_BufferSend, 0, 0, 0, 0} };
+
+static unsigned char UART2_BufferRecv[UART_BuferSize], UART2_BufferSend[UART_BuferSize];
+static UART_Data UART2_Data{ USART2, {sizeof(UART2_BufferRecv), UART2_BufferRecv, 0, 0, 0, 0}, {sizeof(UART2_BufferSend), UART2_BufferSend, 0, 0, 0, 0} };
+
+static unsigned char UART1_BufferRecv[UART_BuferSize], UART1_BufferSend[UART_BuferSize];
+static UART_Data UART1_Data{ USART1, {sizeof(UART1_BufferRecv), UART1_BufferRecv, 0, 0, 0, 0}, {sizeof(UART1_BufferSend), UART1_BufferSend, 0, 0, 0, 0} };
+
+static void IRQHandler(UART_Data& Uart)
+{
+  unsigned long isr = Uart.UART->ISR;
+  if(isr & USART_ISR_RXNE)
+  {
+    Uart.Recv.Buffer[Uart.Recv.Head] = Uart.UART->RDR;
+    if(Uart.Recv.Push-Uart.Recv.Pull<Uart.Recv.Size) 
+    {
+      Uart.Recv.Head++;
+      Uart.Recv.Push++;
+    }
+    if(Uart.Recv.Head>=Uart.Recv.Size) Uart.Recv.Head=0;
+  }
+  if(isr & USART_ISR_TXE)
+  {
+    if(Uart.Send.Push != Uart.Send.Pull)
+    {
+      Uart.UART->TDR = Uart.Send.Buffer[Uart.Send.Tail++];
+      if(Uart.Send.Tail>=Uart.Send.Size) Uart.Send.Tail=0;
+      Uart.Send.Pull++;
+    }
+    else Uart.UART->CR1 &= ~USART_CR1_TXEIE;
+  }
+}
+//------------------------------------------------------------------------------
+
+extern "C" void USART3_IRQHandler()
+{
+  IRQHandler(UART3_Data);
+}
+//------------------------------------------------------------------------------
+
+extern "C" void USART2_IRQHandler()
+{
+  IRQHandler(UART2_Data);
+}
+//------------------------------------------------------------------------------
+
+extern "C" void USART1_IRQHandler()
+{
+  IRQHandler(UART1_Data);
+}
+//------------------------------------------------------------------------------
+
+static void Init(USART_TypeDef* USART, unsigned long Baud)
+{
+  USART->CR1 = 0;
+
+  USART->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_RXNEIE;
+
+  //USART->BRR = SystemCoreClock / 4 / Baud;
+  USART->BRR = SystemCoreClock / Baud;
+
+  USART->CR3 |= USART_CR3_OVRDIS;
+  USART->CR1 |= USART_CR1_UE;
+}
+//------------------------------------------------------------------------------
+
+void UART3_Init(unsigned long Baud)
+{
+  if (RCC->APB1ENR1 & RCC_APB1ENR1_USART3EN) return;
+
+  RCC->APB1ENR1 |= RCC_APB1ENR1_USART3EN;
+
+  GPIO_InitPin(GPIO_PIN_10 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF7 | GPIO_OSPEED_HIGH);
+  GPIO_InitPin(GPIO_PIN_11 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF7 | GPIO_OSPEED_HIGH);
+
+  Init(USART3, Baud);
+
+  NVIC_SetPriority(USART3_IRQn, 0);
+  NVIC_EnableIRQ(USART3_IRQn);
+}
+//------------------------------------------------------------------------------
+
+void UART2_Init(unsigned long Baud)
+{
+  if (RCC->APB1ENR1 & RCC_APB1ENR1_USART2EN) return;
+
+  RCC->APB1ENR1 |= RCC_APB1ENR1_USART2EN;
+
+  GPIO_InitPin(GPIO_PIN_3 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF7 | GPIO_OSPEED_HIGH);
+  GPIO_InitPin(GPIO_PIN_4 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF7 | GPIO_OSPEED_HIGH);
+
+  Init(USART2, Baud);
+
+  NVIC_SetPriority(USART2_IRQn, 0);
+  NVIC_EnableIRQ(USART2_IRQn);
+}
+//------------------------------------------------------------------------------
+
+void UART1_Init(unsigned long Baud)
+{
+  if (RCC->APB2ENR & RCC_APB2ENR_USART1EN) return;
+
+  RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
+
+  GPIO_InitPin(GPIO_PIN_6 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF7 | GPIO_OSPEED_HIGH);
+  GPIO_InitPin(GPIO_PIN_7 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF7 | GPIO_OSPEED_HIGH);
+
+  Init(USART1, Baud);
+
+  NVIC_SetPriority(USART1_IRQn, 0);
+  NVIC_EnableIRQ(USART1_IRQn);
+}
+//------------------------------------------------------------------------------
+
+static unsigned long Recv(UART_Data& Uart, void* Data, unsigned long Size)
+{
+  if(!Data) return Uart.Recv.Push-Uart.Recv.Pull;
+  
+  unsigned char* data=(unsigned char*)Data;
+  unsigned long size = 0;
+  
+  while (size < Size)
+  {
+    if (Uart.Recv.Push == Uart.Recv.Pull) break;
+    data[size++] = Uart.Recv.Buffer[Uart.Recv.Tail++];
+    if(Uart.Recv.Tail>=Uart.Recv.Size) Uart.Recv.Tail=0;
+    Uart.Recv.Pull++;
+  }
+  return size;
+}
+//------------------------------------------------------------------------------
+
+static unsigned long Send(UART_Data& Uart, const void* Data, unsigned long Size)
+{
+  unsigned char* data=(unsigned char*)Data;
+  unsigned long size = 0;
+  
+  while (size < Size)
+  {
+    if (Uart.Send.Push - Uart.Send.Pull >= Uart.Send.Size) break;
+    Uart.Send.Buffer[Uart.Send.Head++] = data[size++];
+    if(Uart.Send.Head>=Uart.Send.Size) Uart.Send.Head=0;
+    Uart.Send.Push++;
+  }
+
+  Uart.UART->CR1 |= USART_CR1_TXEIE;
+
+  return size;
+}
+//------------------------------------------------------------------------------
+
+static inline void Flush(UART_Data& Uart)
+{
+  Uart.Recv.Tail=Uart.Recv.Head;
+  Uart.Recv.Pull=Uart.Recv.Push;
+}
+//------------------------------------------------------------------------------
+
+unsigned long UART3_Recv(void* Data, unsigned long Size)
+{
+  return Recv(UART3_Data, Data, Size);
+}
+//------------------------------------------------------------------------------
+
+void UART3_Flush()
+{
+  Flush(UART3_Data);
+}
+//------------------------------------------------------------------------------
+
+unsigned long UART3_Send(const void* Data, unsigned long Size)
+{
+  return Send(UART3_Data, Data, Size);
+}
+//------------------------------------------------------------------------------
+
+unsigned long UART2_Recv(void* Data, unsigned long Size)
+{
+  return Recv(UART2_Data, Data, Size);
+}
+//------------------------------------------------------------------------------
+
+void UART2_Flush()
+{
+  Flush(UART2_Data);
+}
+//------------------------------------------------------------------------------
+
+unsigned long UART2_Send(const void* Data, unsigned long Size)
+{
+  return Send(UART2_Data, Data, Size);
+}
+//------------------------------------------------------------------------------
+
+unsigned long UART1_Recv(void* Data, unsigned long Size)
+{
+  return Recv(UART1_Data, Data, Size);
+}
+//------------------------------------------------------------------------------
+
+void UART1_Flush()
+{
+  Flush(UART1_Data);
+}
+//------------------------------------------------------------------------------
+
+unsigned long UART1_Send(const void* Data, unsigned long Size)
+{
+  return Send(UART1_Data, Data, Size);
+}
+//------------------------------------------------------------------------------

drv/uart.h

@@ -0,0 +1,16 @@
+#pragma once
+
+void UART3_Init(unsigned long Baud);
+void UART3_Flush();
+unsigned long UART3_Recv(void* Data, unsigned long Size);
+unsigned long UART3_Send(const void* Data, unsigned long Size);
+
+void UART2_Init(unsigned long Baud);
+void UART2_Flush();
+unsigned long UART2_Recv(void* Data, unsigned long Size);
+unsigned long UART2_Send(const void* Data, unsigned long Size);
+
+void UART1_Init(unsigned long Baud);
+void UART1_Flush();
+unsigned long UART1_Recv(void* Data, unsigned long Size);
+unsigned long UART1_Send(const void* Data, unsigned long Size);

drv/vl53l0x.h

@@ -0,0 +1,186 @@
+#ifndef VL53L0X_h
+#define VL53L0X_h
+
+#include <cstdint>
+#include "tick.h"
+
+class VL53L0X
+{
+  public:
+    // register addresses from API vl53l0x_device.h (ordered as listed there)
+    enum regAddr
+    {
+      SYSRANGE_START                              = 0x00,
+
+      SYSTEM_THRESH_HIGH                          = 0x0C,
+      SYSTEM_THRESH_LOW                           = 0x0E,
+
+      SYSTEM_SEQUENCE_CONFIG                      = 0x01,
+      SYSTEM_RANGE_CONFIG                         = 0x09,
+      SYSTEM_INTERMEASUREMENT_PERIOD              = 0x04,
+
+      SYSTEM_INTERRUPT_CONFIG_GPIO                = 0x0A,
+
+      GPIO_HV_MUX_ACTIVE_HIGH                     = 0x84,
+
+      SYSTEM_INTERRUPT_CLEAR                      = 0x0B,
+
+      RESULT_INTERRUPT_STATUS                     = 0x13,
+      RESULT_RANGE_STATUS                         = 0x14,
+
+      RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN       = 0xBC,
+      RESULT_CORE_RANGING_TOTAL_EVENTS_RTN        = 0xC0,
+      RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF       = 0xD0,
+      RESULT_CORE_RANGING_TOTAL_EVENTS_REF        = 0xD4,
+      RESULT_PEAK_SIGNAL_RATE_REF                 = 0xB6,
+
+      ALGO_PART_TO_PART_RANGE_OFFSET_MM           = 0x28,
+
+      I2C_SLAVE_DEVICE_ADDRESS                    = 0x8A,
+
+      MSRC_CONFIG_CONTROL                         = 0x60,
+
+      PRE_RANGE_CONFIG_MIN_SNR                    = 0x27,
+      PRE_RANGE_CONFIG_VALID_PHASE_LOW            = 0x56,
+      PRE_RANGE_CONFIG_VALID_PHASE_HIGH           = 0x57,
+      PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT          = 0x64,
+
+      FINAL_RANGE_CONFIG_MIN_SNR                  = 0x67,
+      FINAL_RANGE_CONFIG_VALID_PHASE_LOW          = 0x47,
+      FINAL_RANGE_CONFIG_VALID_PHASE_HIGH         = 0x48,
+      FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44,
+
+      PRE_RANGE_CONFIG_SIGMA_THRESH_HI            = 0x61,
+      PRE_RANGE_CONFIG_SIGMA_THRESH_LO            = 0x62,
+
+      PRE_RANGE_CONFIG_VCSEL_PERIOD               = 0x50,
+      PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI          = 0x51,
+      PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO          = 0x52,
+
+      SYSTEM_HISTOGRAM_BIN                        = 0x81,
+      HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT       = 0x33,
+      HISTOGRAM_CONFIG_READOUT_CTRL               = 0x55,
+
+      FINAL_RANGE_CONFIG_VCSEL_PERIOD             = 0x70,
+      FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI        = 0x71,
+      FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO        = 0x72,
+      CROSSTALK_COMPENSATION_PEAK_RATE_MCPS       = 0x20,
+
+      MSRC_CONFIG_TIMEOUT_MACROP                  = 0x46,
+
+      SOFT_RESET_GO2_SOFT_RESET_N                 = 0xBF,
+      IDENTIFICATION_MODEL_ID                     = 0xC0,
+      IDENTIFICATION_REVISION_ID                  = 0xC2,
+
+      OSC_CALIBRATE_VAL                           = 0xF8,
+
+      GLOBAL_CONFIG_VCSEL_WIDTH                   = 0x32,
+      GLOBAL_CONFIG_SPAD_ENABLES_REF_0            = 0xB0,
+      GLOBAL_CONFIG_SPAD_ENABLES_REF_1            = 0xB1,
+      GLOBAL_CONFIG_SPAD_ENABLES_REF_2            = 0xB2,
+      GLOBAL_CONFIG_SPAD_ENABLES_REF_3            = 0xB3,
+      GLOBAL_CONFIG_SPAD_ENABLES_REF_4            = 0xB4,
+      GLOBAL_CONFIG_SPAD_ENABLES_REF_5            = 0xB5,
+
+      GLOBAL_CONFIG_REF_EN_START_SELECT           = 0xB6,
+      DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD         = 0x4E,
+      DYNAMIC_SPAD_REF_EN_START_OFFSET            = 0x4F,
+      POWER_MANAGEMENT_GO1_POWER_FORCE            = 0x80,
+
+      VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV           = 0x89,
+
+      ALGO_PHASECAL_LIM                           = 0x30,
+      ALGO_PHASECAL_CONFIG_TIMEOUT                = 0x30,
+    };
+
+    enum vcselPeriodType { VcselPeriodPreRange, VcselPeriodFinalRange };
+
+    uint8_t last_status; // status of last I2C transmission
+
+    VL53L0X();
+
+
+
+    void setAddress(uint8_t new_addr);
+    inline uint8_t getAddress() { return address; }
+
+    bool init(bool io_2v8 = true);
+
+    void writeReg(uint8_t reg, uint8_t value);
+    void writeReg16Bit(uint8_t reg, uint16_t value);
+    void writeReg32Bit(uint8_t reg, uint32_t value);
+    uint8_t readReg(uint8_t reg);
+    uint16_t readReg16Bit(uint8_t reg);
+    uint32_t readReg32Bit(uint8_t reg);
+
+    void writeMulti(uint8_t reg, uint8_t const * src, uint8_t count);
+    void readMulti(uint8_t reg, uint8_t * dst, uint8_t count);
+
+    bool setSignalRateLimit(float limit_Mcps);
+    float getSignalRateLimit();
+
+    bool setMeasurementTimingBudget(uint32_t budget_us);
+    uint32_t getMeasurementTimingBudget();
+
+    bool setVcselPulsePeriod(vcselPeriodType type, uint8_t period_pclks);
+    uint8_t getVcselPulsePeriod(vcselPeriodType type);
+
+    void startContinuous(uint32_t period_ms = 0);
+    void stopContinuous();
+    uint16_t readRangeContinuousMillimeters();
+    uint16_t readRangeSingleMillimeters();
+    
+    
+    bool isRangeReady();
+    uint16_t readRange();
+
+    inline void setTimeout(uint16_t timeout) { io_timeout = timeout; }
+    inline uint16_t getTimeout() { return io_timeout; }
+    bool timeoutOccurred();
+
+  private:
+    // TCC: Target CentreCheck
+    // MSRC: Minimum Signal Rate Check
+    // DSS: Dynamic Spad Selection
+
+    struct SequenceStepEnables
+    {
+      bool tcc, msrc, dss, pre_range, final_range;
+    };
+
+    struct SequenceStepTimeouts
+    {
+      uint16_t pre_range_vcsel_period_pclks, final_range_vcsel_period_pclks;
+
+      uint16_t msrc_dss_tcc_mclks, pre_range_mclks, final_range_mclks;
+      uint32_t msrc_dss_tcc_us,    pre_range_us,    final_range_us;
+    };
+
+   
+    uint8_t address;
+    uint16_t io_timeout;
+    bool did_timeout;
+    uint16_t timeout_start_ms;
+
+    uint8_t stop_variable; // read by init and used when starting measurement; is StopVariable field of VL53L0X_DevData_t structure in API
+    uint32_t measurement_timing_budget_us;
+
+    bool getSpadInfo(uint8_t * count, bool * type_is_aperture);
+
+    void getSequenceStepEnables(SequenceStepEnables * enables);
+    void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts);
+
+    bool performSingleRefCalibration(uint8_t vhv_init_byte);
+
+    static uint16_t decodeTimeout(uint16_t value);
+    static uint16_t encodeTimeout(uint32_t timeout_mclks);
+    static uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks);
+    static uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks);
+    
+   
+
+};
+
+#endif
+
+

main.cpp

@@ -0,0 +1,587 @@
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "stm32g4xx.h"
+
+#include "gpio.h"
+#include "led.h"
+#include "ori.h"
+#include "bar.h"
+#include "eep.h"
+#include "tim.h"
+#include "pwm.h"
+#include "tele.h"
+#include "med.h"
+#include "tick.h"
+
+#include "sbus.h"
+#include "iwdg.h"
+#include "pid.h"
+#include "uart.h"
+#include "gps.h"
+
+#include "laser.h"
+#include "flow.h"
+
+#include "imu.h"
+
+#include "mot.h"
+#include "move.h"
+
+#include "prot.h"
+
+
+static const float PI = 3.14159265359f;
+static const float TO_RAD = PI/180.0f;
+
+#define TELE_TIMER 10 // ms
+#define LED_TIMER 100 // ms
+#define JOY_TIMER 200 // ms
+
+#define LOG_TIMER 5 // ms
+
+#define PWM_FREQ 100 // Hz
+
+#define MAIN_UPDATE_FREQ 100 // Hz
+#define MAIN_UPDATE_PRIORITY 2
+
+PID_Data PID_X = { -1000, 1000, {-500, 500, 1}, {-50, 50, 0.0}, {-500, 500, 0.020} };
+PID_Data PID_Y = { -1000, 1000, {-500, 500, 1}, {-50, 50, 0.0}, {-500, 500, 0.020} };
+PID_Data PID_Z = { -1000, 1000, {-500, 500, 1}, {-50, 50, 0.0}, {-500, 500, 0.03}  };
+
+short Joy_throt = 0; // Gass
+short Joy_pitch = 0; // Y
+short Joy_roll = 0;  // X
+short Joy_yaw = 0;   // Z
+
+short Main_PWM[4]{ 0, 0, 0, 0 };
+
+float Main_dXYZ[3]={0, 0, 0};
+bool MainValid=false;
+
+//IBUS_Data Joypad;
+SBUS_Data Joypad;
+bool JoypadOff;
+
+IMU_Data DataIMU;
+BAR_Data DataBAR;
+MAG_Data DataMAG = {0,0,0};
+
+
+bool imu_ready=false, bar_ready=false, mag_ready=false;
+
+
+
+SBUS_Data rc;
+unsigned char fs;
+
+short PWM_NONE = 0;
+
+
+int  m= 0;
+unsigned short dist_mm;
+      uint16_t dist ;
+        float meters;
+
+    short dx, dy;
+    unsigned char qality;
+ 
+
+void DoneProcIMU(IMU_Data& Data)
+{
+  DataIMU=Data;
+  imu_ready=true;
+  if(bar_ready && mag_ready) NVIC_SetPendingIRQ(TIM7_IRQn);
+}
+
+void DoneProcBAR(bool Ready, BAR_Data& Data)
+{
+  //if(!Ready) { BAR_GetAsunc(DoneProcBAR); return; }
+  DataBAR=Data;
+  bar_ready=true;
+  if(imu_ready && mag_ready) NVIC_SetPendingIRQ(TIM7_IRQn);
+}
+
+
+
+short test_pow[3];
+
+Point DataGPS{0,0,0};
+
+short test_p=0, test_r=0;
+
+Vec3 test_iner={0,0,0};
+
+float range = 0;
+
+ void Main_Update()
+{
+ IMU_Get(DataIMU);
+      
+   DataIMU.Bar=BAR_GetData(&DataIMU.Tmp);
+  //if(!mag_ready) MAG_GetAsunc(DoneProcMAG);
+
+  //if(!imu_ready || !bar_ready || !mag_ready) return;
+//if(!imu_ready || !bar_ready) return;
+  
+  
+  
+  PingIWDG();
+  
+  imu_ready=false;
+  bar_ready=false;
+  mag_ready=false;
+  
+  DataIMU.Mag={DataMAG.X, DataMAG.Y, DataMAG.Z};
+  
+  ORI_Get(DataORI, MAIN_UPDATE_FREQ, DataIMU, DataBAR);
+  
+  static Vec3 flow;
+  
+  static long timer=0;
+  if(timer>=4)
+  {
+     if (isRangeReady())
+    {
+     range = getRange();
+    }
+
+    bool move=FLOW_GetMotion(&dx, &dy, &qality);
+    flow=Motion({DataORI.SinX, DataORI.SinY, DataORI.CosZ}, move, dx, dy, range, 0);
+    timer=0;
+    
+    if(range<0.15f) 
+    {
+      Main_StatusMode=STATUS_MODE::on_ground;
+      Main_SysMode=SYS_MODE::hold;
+    }
+  }
+  else timer++;
+  
+  short yaw=(DataORI.Yaw-Joy_yaw)+180;
+  if(yaw<0) yaw+=360;
+  if(yaw>360) yaw-=360;
+  static short pov_z;
+  pov_z = (short)(PID_Update(180, yaw, PID_Z, DataORI.Gyr.Z));
+  
+  static float height;
+  static Point zero;
+  
+  
+  float altitude;
+  Point coord;
+  bool valid;
+  bool ready;
+  
+  ready=GPS_GetCoordinates(DataGPS, valid);
+  valid=1;
+  
+  /*MainValid=(last && valid);
+  
+  if(MainValid) 
+  {
+    if(Joypad.SWB>JOY_MID) { zero=coord; height=altitude; ready=true; }
+    GPS_LocalDistance(zero, coord, Main_dXYZ[0], Main_dXYZ[1]);
+    Main_dXYZ[2]=height-altitude;
+  }*/
+  
+  //GPS_Navigation(MainValid, DataORI, Main_dXYZ);
+  
+  //if(Joypad.SWA<JOY_MID ) return;
+  
+  static float heigh=0, heigh_g=0, heigh_z=0;
+  
+  if(Joypad.SWA<JOY_MID)
+  {
+    PWM_SetAll(PWM_NONE);
+    Base_BeginXYZ=DataGPS;
+    return;
+  }
+  
+  if(Joy_throt<JOY_MIN+50)
+  {
+    PWM_SetAll(PWM_NONE);
+    return;
+  }
+  
+  static bool autopilot=false;
+  
+  static short throt;
+  throt = (Joy_throt-JOY_MIN)/JOY_VAL;
+  
+  static short pitch=0, roll=0;
+  
+  
+  
+  if(Joypad.X>JOY_MID+50 || Joypad.X<JOY_MID-50 || Joypad.Y>JOY_MID+50 || Joypad.Y<JOY_MID-50) autopilot=false;
+  
+  static float mx=0, my=0;
+  
+  if(Joypad.SWD>JOY_MID)
+  {
+    MainFlyMode=MAIN_FLY_MODE::STOP;
+    
+    /*if(Joypad.SWB>JOY_MID)
+    {
+      MainFlyMode=MAIN_FLY_MODE::STOP;
+    }
+    else
+    {
+      //PointGPS=DataGPS;
+      //PointGPS.Altitude+=3;
+    }*/
+    
+    if(MainFlyMode==MAIN_FLY_MODE::WAIT)
+    {
+      if(autopilot==true) { PWM_SetAll(900); return; }
+    }
+    
+    if(MainFlyMode==MAIN_FLY_MODE::START)
+    {
+      if(autopilot==true) { PWM_SetAll(1100); return; }
+    }
+    
+    if(MainFlyMode==MAIN_FLY_MODE::STOP)
+    {
+      Main_StatusMode=STATUS_MODE::landing;
+      Main_SysMode=SYS_MODE::land;
+      
+      if(autopilot==true)
+      {
+        //throt=500;
+        
+        float shift=0.5f+flow.z;
+        
+        float fx=-flow.x*shift;
+        float fy=-flow.y*shift;
+        
+        mx+=fx;
+        my+=fy;
+        
+        test_iner.x=mx/1000;
+        test_iner.y=my/1000;
+        test_iner.z=flow.z;
+        
+        pitch=my*0.005f;
+        roll=mx*0.005f;
+        
+        pitch+=fy*0.4f;
+        roll+=fx*0.4f;
+        
+        if(pitch>+20) pitch=+20;
+        if(pitch<-20) pitch=-20;
+        
+        if(roll>+20) roll=+20;
+        if(roll<-20) roll=-20;
+        
+        Joy_pitch=pitch;
+        Joy_roll=roll;
+        
+        static long add_power=0;
+        add_power=(heigh-flow.z*1000.0f)*0.1f;
+        
+        if(add_power>+500) add_power=+500;
+        if(add_power<-500) add_power=-500;
+        
+        throt+=add_power;
+        
+        static int count=0;
+        static float last_h=0, delta_h=0;
+        if(count>=9)
+        {
+          delta_h=(last_h-flow.z*1000.0f);
+          
+          last_h=flow.z*1000.0f;
+          
+          count=0;
+        }
+        else count++;
+        
+        if(delta_h>+200) delta_h=+200;
+        if(delta_h<-200) delta_h=-200;
+        
+        throt+=delta_h*1.0f;
+        
+        if(throt>800) throt=800;
+        if(throt<100) throt=100;
+        
+
+        if(Joypad.SWB>JOY_MID)
+        {
+          if(flow.z>0.3f) 
+            heigh-=3.0f;
+          else 
+            heigh-=10.0f;
+        }
+        if(heigh<-5000) 
+        {
+          heigh=-5000;
+          MainFlyMode=MAIN_FLY_MODE::START;
+        }
+      }
+    }
+    else 
+    {
+      heigh=flow.z*1000.0f;
+      mx=0; my=0;
+    }
+    
+    if(MainFlyMode==MAIN_FLY_MODE::FLY)
+    {
+
+      Main_StatusMode=STATUS_MODE::fly;
+      Main_SysMode=SYS_MODE::auto_mode;
+      
+      if((autopilot==true) && (ready && valid))
+      {
+        //throt=500;
+        
+        static float last_a=0, delta_a=0;
+        long power=(PointGPS.Altitude-DataGPS.Altitude)*100.0f;
+        
+        
+        
+        static int count=0;
+        
+        if(count>1)
+        {
+          delta_a=(DataGPS.Altitude-last_a);
+          
+          last_a=DataGPS.Altitude;
+          
+          count=0;
+        }
+        else count++;
+        
+        power+=delta_a*50.0f;
+        
+        if(power>+500) power=+500;
+        if(power<-500) power=-500;
+        
+        //throt+=power;
+        
+        //if(throt<100) throt=100;
+      }
+      
+      if((autopilot==true) && (ready && valid))
+      {
+        static float delta_x=0, delta_y=0;
+        static float ldx=0, ldy=0;
+        
+        float dx, dy;
+        
+        GPS_LocalDistance(PointGPS, DataGPS, dx, dy);
+        
+        dx=dx;
+        dy=-dy;
+        
+        
+        pitch=dy*10.0f;
+        roll=dx*10.0f;
+        
+        if(pitch>+20) pitch=+20;
+        if(pitch<-20) pitch=-20;
+        
+        if(roll>+20) roll=+20;
+        if(roll<-20) roll=-20;
+        
+        static int count=0;
+        
+        if(count>1)
+        {
+          delta_x=(delta_x+(dx-ldx))/2.0f;
+          delta_y=(delta_y+(dy-ldy))/2.0f;
+          
+          ldx=dx;
+          ldy=dy;
+          
+          count=0;
+        }
+        else count++;
+        
+        //pitch+=delta_y*10.0f;
+        //roll+=delta_x*10.0f;
+        
+        if(pitch>+20) pitch=+20;
+        if(pitch<-20) pitch=-20;
+        
+        if(roll>+20) roll=+20;
+        if(roll<-20) roll=-20;
+        
+        test_p=pitch;
+        test_r=roll;
+        
+        //Vec3 v = RotateToZ({(float)pitch, (float)roll, 0}, false);
+        //pitch=v.x; roll=v.y;
+        
+        Joy_pitch=pitch;
+        Joy_roll=roll;
+      }
+      
+      if(autopilot==true)
+      {
+        Joy_pitch=test_p;
+        Joy_roll=test_r;
+      }
+    }
+  }
+  else
+  {
+    mx=0; my=0;
+    heigh=flow.z*1000.0f;
+    MainFlyMode=MAIN_FLY_MODE::WAIT;
+    autopilot=true;
+  }
+  
+  short pov_x = (short)(PID_Update(Joy_pitch, DataORI.Pitch, PID_X, -DataORI.Gyr.X));
+  short pov_y = (short)(PID_Update(Joy_roll,  DataORI.Roll,  PID_Y, -DataORI.Gyr.Y));
+  
+  //throt+=pov_h;
+  
+  //float stream=DataORI.Tilt;
+  //if(stream>25) stream=25;
+  //throt*=1.0f/cosf(stream*TO_RAD);
+  if(throt>1000) throt=1000;
+  
+  QUAD_Set(throt, pov_x, pov_y, pov_z); //!!!!!!!!!!!!!!!!
+}
+//------------------------------------------------------------------------------
+ 
+ float poww=0;
+ bool isGoZero = false;
+ 
+ 
+int main()
+{
+  //GPIO_InitPin(GPIO_PIN_13 | GPIO_PORT_C | GPIO_OUTPUT); // POWER ON 3V3
+  
+ 
+  CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+  DWT->CYCCNT = 0;
+  DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+  
+  //asm("BKPT 0");
+  
+   PWM_Init(PWM_FREQ); //???
+   PWM_SetAll(PWM_NONE);//???
+   
+   LED_Init();
+   
+   
+   ORI_Init();
+   BAR_Init();
+   
+ //  EEP_Init();
+   TIM7_Init(MAIN_UPDATE_PRIORITY);
+  TICK_Init();
+  SBUS_Init(); 
+  Laser_Init_v2(); 
+  UART2_Init(57600);//LPUART1_Init
+  
+  FLOW_Init();
+  //EEP_Write(0, "Hello", 6);
+  //char data[32];
+  //EEP_Read(2, data, 32);
+  
+    TIM7_Update(MAIN_UPDATE_FREQ*2, Main_Update); 
+
+   //174 1811
+  while(true)
+  {
+   // GPS_Update();
+   
+    /*
+     if(SBUS_Update(Joypad, JoypadOff))
+     {
+         poww = (Joypad.Z-174)*700/(1811-174);
+        PWM_SetAll(PWM_NONE);
+     }
+    */
+    
+    
+    PROT_Update();
+    
+    const unsigned long tick=TICK_GetCount();
+    
+    static unsigned long joy_tick=tick;
+    
+    
+
+
+  //--------------------------------------------------
+    
+    if(SBUS_Update(Joypad, JoypadOff))
+    {
+      joy_tick=tick;
+      if(Joypad.SWA>JOY_MID)
+      {
+        if (!isGoZero)
+        {
+          GoZero();
+          isGoZero = true;
+        }
+        Joy_pitch = (JOY_MID_PITCH - Joypad.Y)/16;
+        Joy_roll = (Joypad.X - JOY_MID_ROLL)/16;
+        Joy_yaw = (Joypad.W - JOY_MID_YAW)/16;
+        
+        if (Joypad.Z < JOY_MIN+50) Joy_throt = JOY_MIN;
+        else if (Joypad.Z >= JOY_MIN+50 && Joypad.Z <= JOY_MAX)
+        {
+          Joy_throt = Joypad.Z;
+        }
+      }
+      else
+      {
+        PWM_SetAll(PWM_NONE);
+             isGoZero = false;
+      }
+    }
+   
+    //--------------------------------------------------
+    if((tick-joy_tick>JOY_TIMER) || JoypadOff)
+    {
+      PWM_SetAll(PWM_NONE);
+    }
+    
+    static unsigned long led_tick=0;
+    static bool led=false;
+    if(tick-led_tick>LED_TIMER)
+    {
+      led_tick=tick;
+      LED_Set(led=!led);
+      
+    }
+       
+    //short info[]={DataORI.Acc.X, DataORI.Acc.Y, DataORI.Acc.Z, DataORI.Acc.Sum, DataORI.Gyr.X, DataORI.Gyr.Y, DataORI.Gyr.Z, (short)DataORI.Pitch, (short)DataORI.Roll, (short)DataORI.Yaw};
+    //TELE_Update(info, sizeof(info), TELE_TIMER);
+    
+    static int Fixed=0;
+    
+    static unsigned long log_tick=0;
+    if(tick-log_tick>TELE_TIMER)
+    {
+      log_tick=tick;
+//      
+//      float info[]={DataORI.Bar.Acc, DataORI.Speed.Z, DataORI.Bar.Speed};
+//      UART2_Send("Z", 1);
+//      UART2_Send(info, sizeof(info));
+//      UART2_Send("V", 1);
+//      
+//      static char buffer[128];
+      //int len=sprintf(buffer, "Z:%d,%d,%d,%d,%d,%d,%d#\n", Fixed, (int)DataORI.Pitch, (int)DataORI.Roll, (int)DataORI.Yaw, (int)(DataORI.Iner.X*1000), (int)(DataORI.Iner.Y*1000), (int)(DataORI.Iner.Z*1000)); 
+
+      //if(len>0) LPUART1_Send(buffer, len);
+      
+      //int len=sprintf(buffer, "G:0,%4.3f,%4.8f,%4.8f#\n", DataGPS.Altitude, DataGPS.Coord.Latitude, DataGPS.Coord.Longitude);
+      
+      //int len=sprintf(buffer, "G:0,%d,%d\n", test_p, test_r);
+      
+      //if(len>0) LPUART1_Send(buffer, len);
+      
+      Fixed++;
+    }
+  }
+}
+//------------------------------------------------------------------------------
+  

settings/Fly.Debug.cspy.bat

@@ -0,0 +1,40 @@
+@REM This batch file has been generated by the IAR Embedded Workbench
+@REM C-SPY Debugger, as an aid to preparing a command line for running
+@REM the cspybat command line utility using the appropriate settings.
+@REM
+@REM Note that this file is generated every time a new debug session
+@REM is initialized, so you may want to move or rename the file before
+@REM making changes.
+@REM
+@REM You can launch cspybat by typing the name of this batch file followed
+@REM by the name of the debug file (usually an ELF/DWARF or UBROF file).
+@REM
+@REM Read about available command line parameters in the C-SPY Debugging
+@REM Guide. Hints about additional command line parameters that may be
+@REM useful in specific cases:
+@REM   --download_only   Downloads a code image without starting a debug
+@REM                     session afterwards.
+@REM   --silent          Omits the sign-on message.
+@REM   --timeout         Limits the maximum allowed execution time.
+@REM 
+
+
+@echo off 
+
+if not "%~1" == "" goto debugFile 
+
+@echo on 
+
+"C:\iar\ewarm-9.60.3\common\bin\cspybat" -f "D:\Works\GIT\SKB reps\NewProc\Fly IMU\Fly IMU\settings\Fly.Debug.general.xcl" --backend -f "D:\Works\GIT\SKB reps\NewProc\Fly IMU\Fly IMU\settings\Fly.Debug.driver.xcl" 
+
+@echo off 
+goto end 
+
+:debugFile 
+
+@echo on 
+
+"C:\iar\ewarm-9.60.3\common\bin\cspybat" -f "D:\Works\GIT\SKB reps\NewProc\Fly IMU\Fly IMU\settings\Fly.Debug.general.xcl" "--debug_file=%~1" --backend -f "D:\Works\GIT\SKB reps\NewProc\Fly IMU\Fly IMU\settings\Fly.Debug.driver.xcl" 
+
+@echo off 
+:end

settings/Fly.Debug.cspy.ps1

@@ -0,0 +1,31 @@
+param([String]$debugfile = "");
+
+# This powershell file has been generated by the IAR Embedded Workbench
+# C - SPY Debugger, as an aid to preparing a command line for running
+# the cspybat command line utility using the appropriate settings.
+#
+# Note that this file is generated every time a new debug session
+# is initialized, so you may want to move or rename the file before
+# making changes.
+#
+# You can launch cspybat by typing Powershell.exe -File followed by the name of this batch file, followed
+# by the name of the debug file (usually an ELF / DWARF or UBROF file).
+#
+# Read about available command line parameters in the C - SPY Debugging
+# Guide. Hints about additional command line parameters that may be
+# useful in specific cases :
+#   --download_only   Downloads a code image without starting a debug
+#                     session afterwards.
+#   --silent          Omits the sign - on message.
+#   --timeout         Limits the maximum allowed execution time.
+#
+
+
+if ($debugfile -eq "")
+{
+& "C:\iar\ewarm-9.60.3\common\bin\cspybat" -f "D:\Works\GIT\SKB reps\NewProc\Fly IMU\Fly IMU\settings\Fly.Debug.general.xcl" --backend -f "D:\Works\GIT\SKB reps\NewProc\Fly IMU\Fly IMU\settings\Fly.Debug.driver.xcl" 
+}
+else
+{
+& "C:\iar\ewarm-9.60.3\common\bin\cspybat" -f "D:\Works\GIT\SKB reps\NewProc\Fly IMU\Fly IMU\settings\Fly.Debug.general.xcl" --debug_file=$debugfile --backend -f "D:\Works\GIT\SKB reps\NewProc\Fly IMU\Fly IMU\settings\Fly.Debug.driver.xcl" 
+}

settings/Fly.Debug.driver.xcl

@@ -0,0 +1,29 @@
+--endian=little
+
+--cpu=Cortex-M4
+
+--fpu=VFPv4_SP
+
+-p
+
+C:\iar\ewarm-9.60.3\arm\config\debugger\ST\STM32G431CB.ddf
+
+--drv_verify_download
+
+--semihosting=none
+
+--device=STM32G431CB
+
+--drv_interface=SWD
+
+--stlink_reset_strategy=0,0
+
+--drv_swo_clock_setup=16000000,1,2000000
+
+--drv_catch_exceptions=0x000
+
+--drv_debug_ap=0
+
+
+
+

settings/Fly.Debug.general.xcl

@@ -0,0 +1,15 @@
+C:\iar\ewarm-9.60.3\arm\bin\armPROC.dll
+
+ C:\iar\ewarm-9.60.3\arm\bin\armSTLINK.dll
+
+ "D:\Works\GIT\SKB reps\NewProc\Fly IMU\Fly IMU\Debug\Exe\Fly.out"
+
+ --plugin=C:\iar\ewarm-9.60.3\arm\bin\armLibSupportUniversal.dll
+
+ --device_macro=C:\iar\ewarm-9.60.3\arm/config/debugger/ST/STM32G4xx.dmac
+
+ --flash_loader=C:\iar\ewarm-9.60.3\arm/config/flashloader/ST/FlashSTM32G43xxB.board
+
+ 
+
+

settings/Fly.crun

@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<crun>
+    <version>1</version>
+    <filter_entries>
+        <filter index="0" type="default">
+            <type>*</type>
+            <start_file>*</start_file>
+            <end_file>*</end_file>
+            <action_debugger>0</action_debugger>
+            <action_log>1</action_log>
+        </filter>
+    </filter_entries>
+</crun>

settings/Fly.dnx

@@ -0,0 +1,177 @@
+<?xml version="1.0"?>
+<settings>
+    <TerminalIO>
+        <InputSource>1</InputSource>
+        <InputMode2>10</InputMode2>
+        <Filename>$PROJ_DIR$\TermIOInput.txt</Filename>
+        <InputEcho>1</InputEcho>
+        <ShowReset>0</ShowReset>
+        <InputEncodingICU>0</InputEncodingICU>
+        <OutputEncodingICU>0</OutputEncodingICU>
+    </TerminalIO>
+    <Stack>
+        <FillEnabled>0</FillEnabled>
+        <OverflowWarningsEnabled>1</OverflowWarningsEnabled>
+        <WarningThreshold>90</WarningThreshold>
+        <SpWarningsEnabled>1</SpWarningsEnabled>
+        <WarnLogOnly>1</WarnLogOnly>
+        <UseTrigger>1</UseTrigger>
+        <TriggerName>main</TriggerName>
+        <LimitSize>0</LimitSize>
+        <ByteLimit>50</ByteLimit>
+    </Stack>
+    <Interrupts>
+        <Enabled>1</Enabled>
+    </Interrupts>
+    <MemConfig>
+        <Base>1</Base>
+        <Manual>0</Manual>
+        <Ddf>1</Ddf>
+        <TypeViol>0</TypeViol>
+        <Stop>1</Stop>
+    </MemConfig>
+    <Trace1>
+        <Enabled>0</Enabled>
+        <ShowSource>1</ShowSource>
+    </Trace1>
+    <Simulator>
+        <Freq>10000000</Freq>
+        <FreqHi>0</FreqHi>
+        <MultiCoreRunAll>1</MultiCoreRunAll>
+    </Simulator>
+    <StLinkDriver>
+        <stlinkserialNo>16004A002933353739303541</stlinkserialNo>
+        <stlinkfoundProbes />
+        <stlinkResetStyle>0</stlinkResetStyle>
+        <stlinkResetStrategy>0</stlinkResetStrategy>
+        <LeaveTargetRunning>_ 0</LeaveTargetRunning>
+        <CStepIntDis>_ 0</CStepIntDis>
+    </StLinkDriver>
+    <PlDriver>
+        <FirstRun>0</FirstRun>
+        <MemConfigValue>C:\iar\ewarm-9.60.3\arm\config\debugger\ST\STM32G431CB.ddf</MemConfigValue>
+    </PlDriver>
+    <DebugChecksum>
+        <Checksum>2271045821</Checksum>
+    </DebugChecksum>
+    <Exceptions>
+        <StopOnUncaught>_ 0</StopOnUncaught>
+        <StopOnThrow>_ 0</StopOnThrow>
+    </Exceptions>
+    <Disassembly>
+        <MixedMode>1</MixedMode>
+        <InstrCount>0</InstrCount>
+    </Disassembly>
+    <CallStack>
+        <ShowArgs>0</ShowArgs>
+    </CallStack>
+    <SWOTraceHWSettings>
+        <OverrideDefaultClocks>0</OverrideDefaultClocks>
+        <CpuClock>16000000</CpuClock>
+        <ClockAutoDetect>1</ClockAutoDetect>
+        <ClockWanted>2000000</ClockWanted>
+        <JtagSpeed>2000000</JtagSpeed>
+        <Prescaler>8</Prescaler>
+        <TimeStampPrescIndex>0</TimeStampPrescIndex>
+        <TimeStampPrescData>0</TimeStampPrescData>
+        <PcSampCYCTAP>1</PcSampCYCTAP>
+        <PcSampPOSTCNT>15</PcSampPOSTCNT>
+        <PcSampIndex>0</PcSampIndex>
+        <DataLogMode>0</DataLogMode>
+        <ITMportsEnable>0</ITMportsEnable>
+        <ITMportsTermIO>0</ITMportsTermIO>
+        <ITMportsLogFile>0</ITMportsLogFile>
+        <ITMlogFile>$PROJ_DIR$\ITM.log</ITMlogFile>
+    </SWOTraceHWSettings>
+    <ArmDriver>
+        <EnableCache>1</EnableCache>
+        <EnforceMemoryConfiguration>1</EnforceMemoryConfiguration>
+    </ArmDriver>
+    <FlashMemory>
+        <FlashEraseSelection>0 </FlashEraseSelection>
+    </FlashMemory>
+    <InterruptStripe>
+        <Sort>1</Sort>
+    </InterruptStripe>
+    <DataStripe>
+        <Title0>altitude</Title0>
+        <Setup0>4 4 2 0 0 1 0 -100 100</Setup0>
+        <Title1>height</Title1>
+        <Setup1>0 2 1 0 1 0 0 0 4294967295</Setup1>
+    </DataStripe>
+    <watch_formats>
+        <Fmt0>{W}42:isr	4	0</Fmt0>
+    </watch_formats>
+    <Trace2>
+        <Enabled>0</Enabled>
+        <ShowSource>0</ShowSource>
+    </Trace2>
+    <SWOTraceWindow>
+        <PcSampling>0</PcSampling>
+        <InterruptLogs>0</InterruptLogs>
+        <ForcedTimeStamps>0</ForcedTimeStamps>
+        <EventCPI>0</EventCPI>
+        <EventEXC>0</EventEXC>
+        <EventFOLD>0</EventFOLD>
+        <EventLSU>0</EventLSU>
+        <EventSLEEP>0</EventSLEEP>
+    </SWOTraceWindow>
+    <DataLog>
+        <GraphEnabled>0</GraphEnabled>
+        <LogEnabled>1</LogEnabled>
+        <ShowTimeLog>1</ShowTimeLog>
+        <Title_0>altitude</Title_0>
+        <Symbol_0>0 4 0</Symbol_0>
+        <Title_1>height</Title_1>
+        <Symbol_1>0 4 1</Symbol_1>
+        <SumEnabled>0</SumEnabled>
+        <ShowTimeSum>1</ShowTimeSum>
+    </DataLog>
+    <InterruptLog>
+        <GraphEnabled>0</GraphEnabled>
+        <LogEnabled>0</LogEnabled>
+        <ShowTimeLog>1</ShowTimeLog>
+        <SumEnabled>0</SumEnabled>
+        <ShowTimeSum>1</ShowTimeSum>
+        <SumSortOrder>0</SumSortOrder>
+    </InterruptLog>
+    <EventLog>
+        <GraphEnabled>0</GraphEnabled>
+        <LogEnabled>0</LogEnabled>
+        <ShowTimeLog>1</ShowTimeLog>
+        <Title_0>Ch3</Title_0>
+        <Symbol_0>0 0 1</Symbol_0>
+        <Title_1>Ch2</Title_1>
+        <Symbol_1>0 0 1</Symbol_1>
+        <Title_2>Ch1</Title_2>
+        <Symbol_2>0 0 1</Symbol_2>
+        <Title_3>Ch0</Title_3>
+        <Symbol_3>0 0 1</Symbol_3>
+        <SumEnabled>0</SumEnabled>
+        <ShowTimeSum>1</ShowTimeSum>
+        <SumSortOrder>0</SumSortOrder>
+    </EventLog>
+    <DriverProfiling>
+        <Enabled>0</Enabled>
+        <Mode>3</Mode>
+        <Graph>0</Graph>
+        <Symbiont>0</Symbiont>
+        <Exclusions />
+    </DriverProfiling>
+    <TermIOLog>
+        <LoggingEnabled>_ 0</LoggingEnabled>
+        <LogFile>_ ""</LogFile>
+    </TermIOLog>
+    <DisassembleMode>
+        <mode>0</mode>
+    </DisassembleMode>
+    <Breakpoints2>
+        <Bp0>_ 0 "EMUL_CODE" "{$PROJ_DIR$\dev\tof.cpp}.50.9" 0 0 1 "" 0 "" 0</Bp0>
+        <Bp1>_ 0 "EMUL_CODE" "{$PROJ_DIR$\dev\laser.cpp}.55.1" 0 0 1 "" 0 "" 0</Bp1>
+        <Count>2</Count>
+    </Breakpoints2>
+    <Aliases>
+        <Count>0</Count>
+        <SuppressDialog>0</SuppressDialog>
+    </Aliases>
+</settings>

settings/Fly.reggroups

@@ -0,0 +1 @@
+

settings/Fly_EditorBookmarks.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<userBookmarks />

settings/Fly_EncodingOverride.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<userOverrides>
+    <override>
+        <path>C:\Users\Admin\Desktop\Fly IMU\main.cpp</path>
+        <encoding>UTF-8</encoding>
+    </override>
+</userOverrides>

startup_stm32g431xx.s

@@ -0,0 +1,585 @@
+;*******************************************************************************
+;* @File Name          : startup_stm32g431xx.s
+;* @Author             : MCD Application Team
+;* @Brief              : STM32G431xx Devices vector
+;*******************************************************************************
+;* Description        : This module performs:
+;*                      - Set the initial SP
+;*                      - Set the initial PC == _iar_program_start,
+;*                      - Set the vector table entries with the exceptions ISR
+;*                        address.
+;*                      - Branches to main in the C library (which eventually
+;*                        calls main()).
+;*                      After Reset the Cortex-M4 processor is in Thread mode,
+;*                      priority is Privileged, and the Stack is set to Main.
+;********************************************************************************
+;* @attention
+;*
+;* Copyright (c) 2019 STMicroelectronics.
+;* All rights reserved.
+;*
+;* This software is licensed under terms that can be found in the LICENSE file
+;* in the root directory of this software component.
+;* If no LICENSE file comes with this software, it is provided AS-IS.
+;
+;*******************************************************************************
+;
+; The modules in this file are included in the libraries, and may be replaced
+; by any user-defined modules that define the PUBLIC symbol _program_start or
+; a user defined start symbol.
+; To override the cstartup defined in the library, simply add your modified
+; version to the workbench project.
+;
+; The vector table is normally located at address 0.
+; When debugging in RAM, it can be located in RAM, aligned to at least 2^6.
+; The name "__vector_table" has special meaning for C-SPY:
+; it is where the SP start value is found, and the NVIC vector
+; table register (VTOR) is initialized to this address if != 0.
+;
+; Cortex-M version
+;
+
+        MODULE  ?cstartup
+
+        ;; Forward declaration of sections.
+        SECTION CSTACK:DATA:NOROOT(3)
+
+        SECTION .intvec:CODE:NOROOT(2)
+
+        EXTERN  __iar_program_start
+        EXTERN  SystemInit
+        PUBLIC  __vector_table
+
+        DATA
+__vector_table
+        DCD     sfe(CSTACK)
+        DCD     Reset_Handler             ; Reset Handler
+
+        DCD     NMI_Handler               ; NMI Handler
+        DCD     HardFault_Handler         ; Hard Fault Handler
+        DCD     MemManage_Handler         ; MPU Fault Handler
+        DCD     BusFault_Handler          ; Bus Fault Handler
+        DCD     UsageFault_Handler        ; Usage Fault Handler
+        DCD     0                         ; Reserved
+        DCD     0                         ; Reserved
+        DCD     0                         ; Reserved
+        DCD     0                         ; Reserved
+        DCD     SVC_Handler               ; SVCall Handler
+        DCD     DebugMon_Handler          ; Debug Monitor Handler
+        DCD     0                         ; Reserved
+        DCD     PendSV_Handler            ; PendSV Handler
+        DCD     SysTick_Handler           ; SysTick Handler
+
+         ; External Interrupts
+        DCD     WWDG_IRQHandler                   ; Window WatchDog
+        DCD     PVD_PVM_IRQHandler                ; PVD/PVM1/PVM2/PVM3/PVM4 through EXTI Line detection
+        DCD     RTC_TAMP_LSECSS_IRQHandler        ; RTC, TAMP and RCC LSE_CSS through the EXTI line
+        DCD     RTC_WKUP_IRQHandler               ; RTC Wakeup through the EXTI line
+        DCD     FLASH_IRQHandler                  ; FLASH
+        DCD     RCC_IRQHandler                    ; RCC
+        DCD     EXTI0_IRQHandler                  ; EXTI Line0
+        DCD     EXTI1_IRQHandler                  ; EXTI Line1
+        DCD     EXTI2_IRQHandler                  ; EXTI Line2
+        DCD     EXTI3_IRQHandler                  ; EXTI Line3
+        DCD     EXTI4_IRQHandler                  ; EXTI Line4
+        DCD     DMA1_Channel1_IRQHandler          ; DMA1 Channel 1
+        DCD     DMA1_Channel2_IRQHandler          ; DMA1 Channel 2
+        DCD     DMA1_Channel3_IRQHandler          ; DMA1 Channel 3
+        DCD     DMA1_Channel4_IRQHandler          ; DMA1 Channel 4
+        DCD     DMA1_Channel5_IRQHandler          ; DMA1 Channel 5
+        DCD     DMA1_Channel6_IRQHandler          ; DMA1 Channel 6
+        DCD     0                                 ; Reserved
+        DCD     ADC1_2_IRQHandler                 ; ADC1 and ADC2
+        DCD     USB_HP_IRQHandler                 ; USB Device High Priority
+        DCD     USB_LP_IRQHandler                 ; USB Device Low Priority
+        DCD     FDCAN1_IT0_IRQHandler             ; FDCAN1 interrupt line 0
+        DCD     FDCAN1_IT1_IRQHandler             ; FDCAN1 interrupt line 1
+        DCD     EXTI9_5_IRQHandler                ; External Line[9:5]s
+        DCD     TIM1_BRK_TIM15_IRQHandler         ; TIM1 Break, Transition error, Index error and TIM15
+        DCD     TIM1_UP_TIM16_IRQHandler          ; TIM1 Update and TIM16
+        DCD     TIM1_TRG_COM_TIM17_IRQHandler     ; TIM1 Trigger, Commutation, Direction change, Index and TIM17
+        DCD     TIM1_CC_IRQHandler                ; TIM1 Capture Compare
+        DCD     TIM2_IRQHandler                   ; TIM2
+        DCD     TIM3_IRQHandler                   ; TIM3
+        DCD     TIM4_IRQHandler                   ; TIM4
+        DCD     I2C1_EV_IRQHandler                ; I2C1 Event
+        DCD     I2C1_ER_IRQHandler                ; I2C1 Error
+        DCD     I2C2_EV_IRQHandler                ; I2C2 Event
+        DCD     I2C2_ER_IRQHandler                ; I2C2 Error
+        DCD     SPI1_IRQHandler                   ; SPI1
+        DCD     SPI2_IRQHandler                   ; SPI2
+        DCD     USART1_IRQHandler                 ; USART1
+        DCD     USART2_IRQHandler                 ; USART2
+        DCD     USART3_IRQHandler                 ; USART3
+        DCD     EXTI15_10_IRQHandler              ; External Line[15:10]
+        DCD     RTC_Alarm_IRQHandler              ; RTC Alarm (A and B) through EXTI Line
+        DCD     USBWakeUp_IRQHandler              ; USB Wakeup through EXTI line
+        DCD     TIM8_BRK_IRQHandler               ; TIM8 Break, Transition error and Index error Interrupt
+        DCD     TIM8_UP_IRQHandler                ; TIM8 Update Interrupt
+        DCD     TIM8_TRG_COM_IRQHandler           ; TIM8 Trigger, Commutation, Direction change and Index Interrupt
+        DCD     TIM8_CC_IRQHandler                ; TIM8 Capture Compare Interrupt
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     LPTIM1_IRQHandler                 ; LP TIM1 interrupt
+        DCD     0                                 ; Reserved
+        DCD     SPI3_IRQHandler                   ; SPI3
+        DCD     UART4_IRQHandler                  ; UART4
+        DCD     0                                 ; Reserved
+        DCD     TIM6_DAC_IRQHandler               ; TIM6 and DAC1&3 underrun errors
+        DCD     TIM7_IRQHandler                   ; TIM7
+        DCD     DMA2_Channel1_IRQHandler          ; DMA2 Channel 1
+        DCD     DMA2_Channel2_IRQHandler          ; DMA2 Channel 2
+        DCD     DMA2_Channel3_IRQHandler          ; DMA2 Channel 3
+        DCD     DMA2_Channel4_IRQHandler          ; DMA2 Channel 4
+        DCD     DMA2_Channel5_IRQHandler          ; DMA2 Channel 5
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     UCPD1_IRQHandler                  ; UCPD1
+        DCD     COMP1_2_3_IRQHandler              ; COMP1, COMP2 and COMP3
+        DCD     COMP4_IRQHandler                  ; COMP4
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     CRS_IRQHandler                    ; CRS Interrupt
+        DCD     SAI1_IRQHandler                   ; Serial Audio Interface 1 global interrupt
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     FPU_IRQHandler                    ; FPU
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     RNG_IRQHandler                    ; RNG global interrupt
+        DCD     LPUART1_IRQHandler                ; LP UART 1 interrupt
+        DCD     I2C3_EV_IRQHandler                ; I2C3 Event
+        DCD     I2C3_ER_IRQHandler                ; I2C3 Error
+        DCD     DMAMUX_OVR_IRQHandler             ; DMAMUX overrun global interrupt
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     DMA2_Channel6_IRQHandler          ; DMA2 Channel 6
+        DCD     0                                 ; Reserved
+        DCD     0                                 ; Reserved
+        DCD     CORDIC_IRQHandler                 ; CORDIC
+        DCD     FMAC_IRQHandler                   ; FMAC
+
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+;;
+;; Default interrupt handlers.
+;;
+        THUMB
+        PUBWEAK Reset_Handler
+        SECTION .text:CODE:NOROOT:REORDER(2)
+Reset_Handler
+        LDR     R0, =SystemInit
+        BLX     R0
+        LDR     R0, =__iar_program_start
+        BX      R0
+
+        PUBWEAK NMI_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+NMI_Handler
+        B NMI_Handler
+
+        PUBWEAK HardFault_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+HardFault_Handler
+        B HardFault_Handler
+
+        PUBWEAK MemManage_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+MemManage_Handler
+        B MemManage_Handler
+
+        PUBWEAK BusFault_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+BusFault_Handler
+        B BusFault_Handler
+
+        PUBWEAK UsageFault_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+UsageFault_Handler
+        B UsageFault_Handler
+
+        PUBWEAK SVC_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SVC_Handler
+        B SVC_Handler
+
+        PUBWEAK DebugMon_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DebugMon_Handler
+        B DebugMon_Handler
+
+        PUBWEAK PendSV_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+PendSV_Handler
+        B PendSV_Handler
+
+        PUBWEAK SysTick_Handler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SysTick_Handler
+        B SysTick_Handler
+
+        PUBWEAK WWDG_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+WWDG_IRQHandler
+        B WWDG_IRQHandler
+
+        PUBWEAK PVD_PVM_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+PVD_PVM_IRQHandler
+        B PVD_PVM_IRQHandler
+
+        PUBWEAK RTC_TAMP_LSECSS_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+RTC_TAMP_LSECSS_IRQHandler
+        B RTC_TAMP_LSECSS_IRQHandler
+
+        PUBWEAK RTC_WKUP_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+RTC_WKUP_IRQHandler
+        B RTC_WKUP_IRQHandler
+
+        PUBWEAK FLASH_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+FLASH_IRQHandler
+        B FLASH_IRQHandler
+
+        PUBWEAK RCC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+RCC_IRQHandler
+        B RCC_IRQHandler
+
+        PUBWEAK EXTI0_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI0_IRQHandler
+        B EXTI0_IRQHandler
+
+        PUBWEAK EXTI1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI1_IRQHandler
+        B EXTI1_IRQHandler
+
+        PUBWEAK EXTI2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI2_IRQHandler
+        B EXTI2_IRQHandler
+
+        PUBWEAK EXTI3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI3_IRQHandler
+        B EXTI3_IRQHandler
+
+        PUBWEAK EXTI4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI4_IRQHandler
+        B EXTI4_IRQHandler
+
+        PUBWEAK DMA1_Channel1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel1_IRQHandler
+        B DMA1_Channel1_IRQHandler
+
+        PUBWEAK DMA1_Channel2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel2_IRQHandler
+        B DMA1_Channel2_IRQHandler
+
+        PUBWEAK DMA1_Channel3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel3_IRQHandler
+        B DMA1_Channel3_IRQHandler
+
+        PUBWEAK DMA1_Channel4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel4_IRQHandler
+        B DMA1_Channel4_IRQHandler
+
+        PUBWEAK DMA1_Channel5_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel5_IRQHandler
+        B DMA1_Channel5_IRQHandler
+
+        PUBWEAK DMA1_Channel6_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA1_Channel6_IRQHandler
+        B DMA1_Channel6_IRQHandler
+
+        PUBWEAK ADC1_2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+ADC1_2_IRQHandler
+        B ADC1_2_IRQHandler
+
+        PUBWEAK USB_HP_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USB_HP_IRQHandler
+        B USB_HP_IRQHandler
+
+        PUBWEAK USB_LP_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USB_LP_IRQHandler
+        B USB_LP_IRQHandler
+
+        PUBWEAK FDCAN1_IT0_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+FDCAN1_IT0_IRQHandler
+        B FDCAN1_IT0_IRQHandler
+
+        PUBWEAK FDCAN1_IT1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+FDCAN1_IT1_IRQHandler
+        B FDCAN1_IT1_IRQHandler
+
+        PUBWEAK EXTI9_5_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI9_5_IRQHandler
+        B EXTI9_5_IRQHandler
+
+        PUBWEAK TIM1_BRK_TIM15_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM1_BRK_TIM15_IRQHandler
+        B TIM1_BRK_TIM15_IRQHandler
+
+        PUBWEAK TIM1_UP_TIM16_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM1_UP_TIM16_IRQHandler
+        B TIM1_UP_TIM16_IRQHandler
+
+        PUBWEAK TIM1_TRG_COM_TIM17_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM1_TRG_COM_TIM17_IRQHandler
+        B TIM1_TRG_COM_TIM17_IRQHandler
+
+        PUBWEAK TIM1_CC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM1_CC_IRQHandler
+        B TIM1_CC_IRQHandler
+
+        PUBWEAK TIM2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM2_IRQHandler
+        B TIM2_IRQHandler
+
+        PUBWEAK TIM3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM3_IRQHandler
+        B TIM3_IRQHandler
+
+        PUBWEAK TIM4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM4_IRQHandler
+        B TIM4_IRQHandler
+
+        PUBWEAK I2C1_EV_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C1_EV_IRQHandler
+        B I2C1_EV_IRQHandler
+
+        PUBWEAK I2C1_ER_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C1_ER_IRQHandler
+        B I2C1_ER_IRQHandler
+
+        PUBWEAK I2C2_EV_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C2_EV_IRQHandler
+        B I2C2_EV_IRQHandler
+
+        PUBWEAK I2C2_ER_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C2_ER_IRQHandler
+        B I2C2_ER_IRQHandler
+
+        PUBWEAK SPI1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SPI1_IRQHandler
+        B SPI1_IRQHandler
+
+        PUBWEAK SPI2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SPI2_IRQHandler
+        B SPI2_IRQHandler
+
+        PUBWEAK USART1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USART1_IRQHandler
+        B USART1_IRQHandler
+
+        PUBWEAK USART2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USART2_IRQHandler
+        B USART2_IRQHandler
+
+        PUBWEAK USART3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USART3_IRQHandler
+        B USART3_IRQHandler
+
+        PUBWEAK EXTI15_10_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+EXTI15_10_IRQHandler
+        B EXTI15_10_IRQHandler
+
+        PUBWEAK RTC_Alarm_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+RTC_Alarm_IRQHandler
+        B RTC_Alarm_IRQHandler
+
+        PUBWEAK USBWakeUp_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+USBWakeUp_IRQHandler
+        B USBWakeUp_IRQHandler
+
+        PUBWEAK TIM8_BRK_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM8_BRK_IRQHandler
+        B TIM8_BRK_IRQHandler
+
+        PUBWEAK TIM8_UP_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM8_UP_IRQHandler
+        B TIM8_UP_IRQHandler
+
+        PUBWEAK TIM8_TRG_COM_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM8_TRG_COM_IRQHandler
+        B TIM8_TRG_COM_IRQHandler
+
+        PUBWEAK TIM8_CC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM8_CC_IRQHandler
+        B TIM8_CC_IRQHandler
+
+        PUBWEAK LPTIM1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+LPTIM1_IRQHandler
+        B LPTIM1_IRQHandler
+
+        PUBWEAK SPI3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SPI3_IRQHandler
+        B SPI3_IRQHandler
+
+        PUBWEAK UART4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+UART4_IRQHandler
+        B UART4_IRQHandler
+
+        PUBWEAK TIM6_DAC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM6_DAC_IRQHandler
+        B TIM6_DAC_IRQHandler
+
+        PUBWEAK TIM7_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+TIM7_IRQHandler
+        B TIM7_IRQHandler
+
+        PUBWEAK DMA2_Channel1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel1_IRQHandler
+        B DMA2_Channel1_IRQHandler
+
+        PUBWEAK DMA2_Channel2_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel2_IRQHandler
+        B DMA2_Channel2_IRQHandler
+
+        PUBWEAK DMA2_Channel3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel3_IRQHandler
+        B DMA2_Channel3_IRQHandler
+
+        PUBWEAK DMA2_Channel4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel4_IRQHandler
+        B DMA2_Channel4_IRQHandler
+
+        PUBWEAK DMA2_Channel5_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel5_IRQHandler
+        B DMA2_Channel5_IRQHandler
+
+        PUBWEAK UCPD1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+UCPD1_IRQHandler
+        B UCPD1_IRQHandler
+
+        PUBWEAK COMP1_2_3_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+COMP1_2_3_IRQHandler
+        B COMP1_2_3_IRQHandler
+
+        PUBWEAK COMP4_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+COMP4_IRQHandler
+        B COMP4_IRQHandler
+
+        PUBWEAK CRS_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+CRS_IRQHandler
+        B CRS_IRQHandler
+
+        PUBWEAK SAI1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+SAI1_IRQHandler
+        B SAI1_IRQHandler
+
+        PUBWEAK FPU_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+FPU_IRQHandler
+        B FPU_IRQHandler
+
+        PUBWEAK RNG_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+RNG_IRQHandler
+        B RNG_IRQHandler
+
+        PUBWEAK LPUART1_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+LPUART1_IRQHandler
+        B LPUART1_IRQHandler
+
+        PUBWEAK I2C3_EV_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C3_EV_IRQHandler
+        B I2C3_EV_IRQHandler
+
+        PUBWEAK I2C3_ER_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+I2C3_ER_IRQHandler
+        B I2C3_ER_IRQHandler
+
+        PUBWEAK DMAMUX_OVR_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMAMUX_OVR_IRQHandler
+        B DMAMUX_OVR_IRQHandler
+
+        PUBWEAK DMA2_Channel6_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+DMA2_Channel6_IRQHandler
+        B DMA2_Channel6_IRQHandler
+
+        PUBWEAK CORDIC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+CORDIC_IRQHandler
+        B CORDIC_IRQHandler
+
+        PUBWEAK FMAC_IRQHandler
+        SECTION .text:CODE:NOROOT:REORDER(1)
+FMAC_IRQHandler
+        B FMAC_IRQHandler
+
+        END

stm32g431xx_flash.icf

@@ -0,0 +1,36 @@
+/*###ICF### Section handled by ICF editor, don't touch! ****/
+/*-Editor annotation file-*/
+/* IcfEditorFile="$TOOLKIT_DIR$\config\ide\IcfEditor\cortex_v1_0.xml" */
+/*-Specials-*/
+define symbol __ICFEDIT_intvec_start__ = 0x08000000;
+/*-Memory Regions-*/
+define symbol __ICFEDIT_region_ROM_start__     = 0x08000000;
+define symbol __ICFEDIT_region_ROM_end__       = 0x0801FFFF;
+define symbol __ICFEDIT_region_RAM_start__     = 0x20000000;
+define symbol __ICFEDIT_region_RAM_end__       = 0x20007FFF;
+define symbol __ICFEDIT_region_CCMSRAM_start__ = 0x10000000;
+define symbol __ICFEDIT_region_CCMSRAM_end__   = 0x100027FF;
+
+/*-Sizes-*/
+define symbol __ICFEDIT_size_cstack__ = 0x400;
+define symbol __ICFEDIT_size_heap__   = 0x200;
+/**** End of ICF editor section. ###ICF###*/
+
+
+define memory mem with size = 4G;
+define region ROM_region      = mem:[from __ICFEDIT_region_ROM_start__   to __ICFEDIT_region_ROM_end__];
+define region RAM_region      = mem:[from __ICFEDIT_region_RAM_start__   to __ICFEDIT_region_RAM_end__];
+define region CCMSRAM_region  = mem:[from __ICFEDIT_region_CCMSRAM_start__   to __ICFEDIT_region_CCMSRAM_end__];
+
+define block CSTACK    with alignment = 8, size = __ICFEDIT_size_cstack__   { };
+define block HEAP      with alignment = 8, size = __ICFEDIT_size_heap__     { };
+
+initialize by copy { readwrite };
+do not initialize  { section .noinit };
+
+place at address mem:__ICFEDIT_intvec_start__ { readonly section .intvec };
+
+place in ROM_region   { readonly };
+place in RAM_region   { readwrite,
+                        block CSTACK, block HEAP };
+place in CCMSRAM_region { };

stm32g431xx_sram.icf

@@ -0,0 +1,36 @@
+/*###ICF### Section handled by ICF editor, don't touch! ****/
+/*-Editor annotation file-*/
+/* IcfEditorFile="$TOOLKIT_DIR$\config\ide\IcfEditor\cortex_v1_0.xml" */
+/*-Specials-*/
+define symbol __ICFEDIT_intvec_start__ = 0x20000000;
+/*-Memory Regions-*/
+define symbol __ICFEDIT_region_ROM_start__     = 0x20000000;
+define symbol __ICFEDIT_region_ROM_end__       = 0x20001FFF;
+define symbol __ICFEDIT_region_RAM_start__     = 0x20002000;
+define symbol __ICFEDIT_region_RAM_end__       = 0x20007FFF;
+define symbol __ICFEDIT_region_CCMSRAM_start__ = 0x10000000;
+define symbol __ICFEDIT_region_CCMSRAM_end__   = 0x100027FF;
+
+/*-Sizes-*/
+define symbol __ICFEDIT_size_cstack__ = 0x400;
+define symbol __ICFEDIT_size_heap__   = 0x200;
+/**** End of ICF editor section. ###ICF###*/
+
+
+define memory mem with size = 4G;
+define region ROM_region      = mem:[from __ICFEDIT_region_ROM_start__   to __ICFEDIT_region_ROM_end__];
+define region RAM_region      = mem:[from __ICFEDIT_region_RAM_start__   to __ICFEDIT_region_RAM_end__];
+define region CCMSRAM_region  = mem:[from __ICFEDIT_region_CCMSRAM_start__   to __ICFEDIT_region_CCMSRAM_end__];
+
+define block CSTACK    with alignment = 8, size = __ICFEDIT_size_cstack__   { };
+define block HEAP      with alignment = 8, size = __ICFEDIT_size_heap__     { };
+
+initialize by copy { readwrite };
+do not initialize  { section .noinit };
+
+place at address mem:__ICFEDIT_intvec_start__ { readonly section .intvec };
+
+place in ROM_region   { readonly };
+place in RAM_region   { readwrite,
+                        block CSTACK, block HEAP };
+place in CCMSRAM_region { };

stm32g4xx.h

@@ -0,0 +1,259 @@
+/**
+  ******************************************************************************
+  * @file    stm32g4xx.h
+  * @author  MCD Application Team
+  * @brief   CMSIS STM32G4xx Device Peripheral Access Layer Header File.
+  *
+  *          The file is the unique include file that the application programmer
+  *          is using in the C source code, usually in main.c. This file contains:
+  *           - Configuration section that allows to select:
+  *              - The STM32G4xx device used in the target application
+  *              - To use or not the peripheral<61>s drivers in application code(i.e.
+  *                code will be based on direct access to peripheral<61>s registers
+  *                rather than drivers API), this option is controlled by
+  *                "#define USE_HAL_DRIVER"
+  *
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2019 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32g4xx
+  * @{
+  */
+
+#ifndef __STM32G4xx_H
+#define __STM32G4xx_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif /* __cplusplus */
+
+/** @addtogroup Library_configuration_section
+  * @{
+  */
+
+/**
+  * @brief STM32 Family
+  */
+#if !defined (STM32G4)
+#define STM32G4
+#endif /* STM32G4 */
+
+/* Uncomment the line below according to the target STM32G4 device used in your
+   application
+  */
+
+#if !defined (STM32G431xx) && !defined (STM32G441xx) && !defined (STM32G471xx) && \
+    !defined (STM32G473xx) && !defined (STM32G474xx) && !defined (STM32G484xx) && \
+    !defined (STM32GBK1CB) && !defined (STM32G491xx) && !defined (STM32G4A1xx)
+  /* #define STM32G431xx */   /*!< STM32G431xx Devices */
+  /* #define STM32G441xx */   /*!< STM32G441xx Devices */
+  /* #define STM32G471xx */   /*!< STM32G471xx Devices */
+  /* #define STM32G473xx */   /*!< STM32G473xx Devices */
+  /* #define STM32G483xx */   /*!< STM32G483xx Devices */
+  /* #define STM32G474xx */   /*!< STM32G474xx Devices */
+  /* #define STM32G484xx */   /*!< STM32G484xx Devices */
+  /* #define STM32G491xx */   /*!< STM32G491xx Devices */
+  /* #define STM32G4A1xx */   /*!< STM32G4A1xx Devices */
+  /* #define STM32GBK1CB */   /*!< STM32GBK1CB Devices */
+#endif
+
+/*  Tip: To avoid modifying this file each time you need to switch between these
+        devices, you can define the device in your toolchain compiler preprocessor.
+  */
+#if !defined  (USE_HAL_DRIVER)
+/**
+ * @brief Comment the line below if you will not use the peripherals drivers.
+   In this case, these drivers will not be included and the application code will
+   be based on direct access to peripherals registers
+   */
+  /*#define USE_HAL_DRIVER */
+#endif /* USE_HAL_DRIVER */
+
+/**
+  * @brief CMSIS Device version number V1.2.3
+  */
+#define __STM32G4_CMSIS_VERSION_MAIN   (0x01U) /*!< [31:24] main version */
+#define __STM32G4_CMSIS_VERSION_SUB1   (0x02U) /*!< [23:16] sub1 version */
+#define __STM32G4_CMSIS_VERSION_SUB2   (0x03U) /*!< [15:8]  sub2 version */
+#define __STM32G4_CMSIS_VERSION_RC     (0x00U) /*!< [7:0]  release candidate */
+#define __STM32G4_CMSIS_VERSION        ((__STM32G4_CMSIS_VERSION_MAIN << 24)\
+                                       |(__STM32G4_CMSIS_VERSION_SUB1 << 16)\
+                                       |(__STM32G4_CMSIS_VERSION_SUB2 << 8 )\
+                                       |(__STM32G4_CMSIS_VERSION_RC))
+
+/**
+  * @}
+  */
+
+/** @addtogroup Device_Included
+  * @{
+  */
+
+#if defined(STM32G431xx)
+  #include "stm32g431xx.h"
+#elif defined(STM32G441xx)
+  #include "stm32g441xx.h"
+#elif defined(STM32G471xx)
+  #include "stm32g471xx.h"
+#elif defined(STM32G473xx)
+  #include "stm32g473xx.h"
+#elif defined(STM32G483xx)
+  #include "stm32g483xx.h"
+#elif defined(STM32G474xx)
+  #include "stm32g474xx.h"
+#elif defined(STM32G484xx)
+  #include "stm32g484xx.h"
+#elif defined(STM32G491xx)
+  #include "stm32g491xx.h"
+#elif defined(STM32G4A1xx)
+  #include "stm32g4a1xx.h"
+#elif defined(STM32GBK1CB)
+  #include "stm32gbk1cb.h"
+#else
+  #error "Please select first the target STM32G4xx device used in your application (in stm32g4xx.h file)"
+#endif
+
+/**
+  * @}
+  */
+
+/** @addtogroup Exported_types
+  * @{
+  */
+typedef enum
+{
+  RESET = 0,
+  SET = !RESET
+} FlagStatus, ITStatus;
+
+typedef enum
+{
+  DISABLE = 0,
+  ENABLE = !DISABLE
+} FunctionalState;
+#define IS_FUNCTIONAL_STATE(STATE) (((STATE) == DISABLE) || ((STATE) == ENABLE))
+
+typedef enum
+{
+  SUCCESS = 0,
+  ERROR = !SUCCESS
+} ErrorStatus;
+
+/**
+  * @}
+  */
+
+
+/** @addtogroup Exported_macros
+  * @{
+  */
+#define SET_BIT(REG, BIT)     ((REG) |= (BIT))
+
+#define CLEAR_BIT(REG, BIT)   ((REG) &= ~(BIT))
+
+#define READ_BIT(REG, BIT)    ((REG) & (BIT))
+
+#define CLEAR_REG(REG)        ((REG) = (0x0))
+
+#define WRITE_REG(REG, VAL)   ((REG) = (VAL))
+
+#define READ_REG(REG)         ((REG))
+
+#define MODIFY_REG(REG, CLEARMASK, SETMASK)  WRITE_REG((REG), (((READ_REG(REG)) & (~(CLEARMASK))) | (SETMASK)))
+
+#define POSITION_VAL(VAL)     (__CLZ(__RBIT(VAL)))
+
+/* Use of CMSIS compiler intrinsics for register exclusive access */
+/* Atomic 32-bit register access macro to set one or several bits */
+#define ATOMIC_SET_BIT(REG, BIT)                             \
+  do {                                                       \
+    uint32_t val;                                            \
+    do {                                                     \
+      val = __LDREXW((__IO uint32_t *)&(REG)) | (BIT);       \
+    } while ((__STREXW(val,(__IO uint32_t *)&(REG))) != 0U); \
+  } while(0)
+
+/* Atomic 32-bit register access macro to clear one or several bits */
+#define ATOMIC_CLEAR_BIT(REG, BIT)                           \
+  do {                                                       \
+    uint32_t val;                                            \
+    do {                                                     \
+      val = __LDREXW((__IO uint32_t *)&(REG)) & ~(BIT);      \
+    } while ((__STREXW(val,(__IO uint32_t *)&(REG))) != 0U); \
+  } while(0)
+
+/* Atomic 32-bit register access macro to clear and set one or several bits */
+#define ATOMIC_MODIFY_REG(REG, CLEARMSK, SETMASK)                          \
+  do {                                                                     \
+    uint32_t val;                                                          \
+    do {                                                                   \
+      val = (__LDREXW((__IO uint32_t *)&(REG)) & ~(CLEARMSK)) | (SETMASK); \
+    } while ((__STREXW(val,(__IO uint32_t *)&(REG))) != 0U);               \
+  } while(0)
+
+/* Atomic 16-bit register access macro to set one or several bits */
+#define ATOMIC_SETH_BIT(REG, BIT)                            \
+  do {                                                       \
+    uint16_t val;                                            \
+    do {                                                     \
+      val = __LDREXH((__IO uint16_t *)&(REG)) | (BIT);       \
+    } while ((__STREXH(val,(__IO uint16_t *)&(REG))) != 0U); \
+  } while(0)
+
+/* Atomic 16-bit register access macro to clear one or several bits */
+#define ATOMIC_CLEARH_BIT(REG, BIT)                          \
+  do {                                                       \
+    uint16_t val;                                            \
+    do {                                                     \
+      val = __LDREXH((__IO uint16_t *)&(REG)) & ~(BIT);      \
+    } while ((__STREXH(val,(__IO uint16_t *)&(REG))) != 0U); \
+  } while(0)
+
+/* Atomic 16-bit register access macro to clear and set one or several bits */
+#define ATOMIC_MODIFYH_REG(REG, CLEARMSK, SETMASK)                         \
+  do {                                                                     \
+    uint16_t val;                                                          \
+    do {                                                                   \
+      val = (__LDREXH((__IO uint16_t *)&(REG)) & ~(CLEARMSK)) | (SETMASK); \
+    } while ((__STREXH(val,(__IO uint16_t *)&(REG))) != 0U);               \
+  } while(0)
+
+
+/**
+  * @}
+  */
+
+#if defined (USE_HAL_DRIVER)
+ #include "stm32g4xx_hal.h"
+#endif /* USE_HAL_DRIVER */
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* __STM32G4xx_H */
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+
+
+

system_stm32g4xx.c

@@ -0,0 +1,285 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32g4xx.c
+  * @author  MCD Application Team
+  * @brief   CMSIS Cortex-M4 Device Peripheral Access Layer System Source File
+  *
+  *   This file provides two functions and one global variable to be called from
+  *   user application:
+  *      - SystemInit(): This function is called at startup just after reset and
+  *                      before branch to main program. This call is made inside
+  *                      the "startup_stm32g4xx.s" file.
+  *
+  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
+  *                                  by the user application to setup the SysTick
+  *                                  timer or configure other parameters.
+  *
+  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
+  *                                 be called whenever the core clock is changed
+  *                                 during program execution.
+  *
+  *   After each device reset the HSI (16 MHz) is used as system clock source.
+  *   Then SystemInit() function is called, in "startup_stm32g4xx.s" file, to
+  *   configure the system clock before to branch to main program.
+  *
+  *   This file configures the system clock as follows:
+  *=============================================================================
+  *-----------------------------------------------------------------------------
+  *        System Clock source                    | HSI
+  *-----------------------------------------------------------------------------
+  *        SYSCLK(Hz)                             | 16000000
+  *-----------------------------------------------------------------------------
+  *        HCLK(Hz)                               | 16000000
+  *-----------------------------------------------------------------------------
+  *        AHB Prescaler                          | 1
+  *-----------------------------------------------------------------------------
+  *        APB1 Prescaler                         | 1
+  *-----------------------------------------------------------------------------
+  *        APB2 Prescaler                         | 1
+  *-----------------------------------------------------------------------------
+  *        PLL_M                                  | 1
+  *-----------------------------------------------------------------------------
+  *        PLL_N                                  | 16
+  *-----------------------------------------------------------------------------
+  *        PLL_P                                  | 7
+  *-----------------------------------------------------------------------------
+  *        PLL_Q                                  | 2
+  *-----------------------------------------------------------------------------
+  *        PLL_R                                  | 2
+  *-----------------------------------------------------------------------------
+  *        Require 48MHz for RNG                  | Disabled
+  *-----------------------------------------------------------------------------
+  *=============================================================================
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2019 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32g4xx_system
+  * @{
+  */
+
+/** @addtogroup STM32G4xx_System_Private_Includes
+  * @{
+  */
+
+#include "stm32g4xx.h"
+
+#if !defined  (HSE_VALUE)
+  #define HSE_VALUE     24000000U /*!< Value of the External oscillator in Hz */
+#endif /* HSE_VALUE */
+
+#if !defined  (HSI_VALUE)
+  #define HSI_VALUE    16000000U /*!< Value of the Internal oscillator in Hz*/
+#endif /* HSI_VALUE */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_TypesDefinitions
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_Defines
+  * @{
+  */
+
+/************************* Miscellaneous Configuration ************************/
+/* Note: Following vector table addresses must be defined in line with linker
+         configuration. */
+/*!< Uncomment the following line if you need to relocate the vector table
+     anywhere in Flash or Sram, else the vector table is kept at the automatic
+     remap of boot address selected */
+/* #define USER_VECT_TAB_ADDRESS */
+
+#if defined(USER_VECT_TAB_ADDRESS)
+/*!< Uncomment the following line if you need to relocate your vector Table
+     in Sram else user remap will be done in Flash. */
+/* #define VECT_TAB_SRAM */
+#if defined(VECT_TAB_SRAM)
+#define VECT_TAB_BASE_ADDRESS   SRAM_BASE       /*!< Vector Table base address field.
+                                                     This value must be a multiple of 0x200. */
+#define VECT_TAB_OFFSET         0x00000000U     /*!< Vector Table base offset field.
+                                                     This value must be a multiple of 0x200. */
+#else
+#define VECT_TAB_BASE_ADDRESS   FLASH_BASE      /*!< Vector Table base address field.
+                                                     This value must be a multiple of 0x200. */
+#define VECT_TAB_OFFSET         0x00000000U     /*!< Vector Table base offset field.
+                                                     This value must be a multiple of 0x200. */
+#endif /* VECT_TAB_SRAM */
+#endif /* USER_VECT_TAB_ADDRESS */
+/******************************************************************************/
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_Variables
+  * @{
+  */
+  /* The SystemCoreClock variable is updated in three ways:
+      1) by calling CMSIS function SystemCoreClockUpdate()
+      2) by calling HAL API function HAL_RCC_GetHCLKFreq()
+      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
+         Note: If you use this function to configure the system clock; then there
+               is no need to call the 2 first functions listed above, since SystemCoreClock
+               variable is updated automatically.
+  */
+  uint32_t SystemCoreClock = HSI_VALUE;
+
+  const uint8_t AHBPrescTable[16] = {0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U, 6U, 7U, 8U, 9U};
+  const uint8_t APBPrescTable[8] =  {0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U};
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_FunctionPrototypes
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Private_Functions
+  * @{
+  */
+
+/**
+  * @brief  Setup the microcontroller system.
+  * @param  None
+  * @retval None
+  */
+
+void SystemInit(void)
+{
+  /* FPU settings ------------------------------------------------------------*/
+  #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+    SCB->CPACR |= ((3UL << (10*2))|(3UL << (11*2)));  /* set CP10 and CP11 Full Access */
+  #endif
+
+  /* Configure the Vector Table location add offset address ------------------*/
+#if defined(USER_VECT_TAB_ADDRESS)
+  SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
+#endif /* USER_VECT_TAB_ADDRESS */
+}
+
+/**
+  * @brief  Update SystemCoreClock variable according to Clock Register Values.
+  *         The SystemCoreClock variable contains the core clock (HCLK), it can
+  *         be used by the user application to setup the SysTick timer or configure
+  *         other parameters.
+  *
+  * @note   Each time the core clock (HCLK) changes, this function must be called
+  *         to update SystemCoreClock variable value. Otherwise, any configuration
+  *         based on this variable will be incorrect.
+  *
+  * @note   - The system frequency computed by this function is not the real
+  *           frequency in the chip. It is calculated based on the predefined
+  *           constant and the selected clock source:
+  *
+  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(**)
+  *
+  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(***)
+  *
+  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(***)
+  *             or HSI_VALUE(*) multiplied/divided by the PLL factors.
+  *
+  *         (**) HSI_VALUE is a constant defined in stm32g4xx_hal.h file (default value
+  *              16 MHz) but the real value may vary depending on the variations
+  *              in voltage and temperature.
+  *
+  *         (***) HSE_VALUE is a constant defined in stm32g4xx_hal.h file (default value
+  *              24 MHz), user has to ensure that HSE_VALUE is same as the real
+  *              frequency of the crystal used. Otherwise, this function may
+  *              have wrong result.
+  *
+  *         - The result of this function could be not correct when using fractional
+  *           value for HSE crystal.
+  *
+  * @param  None
+  * @retval None
+  */
+void SystemCoreClockUpdate(void)
+{
+  uint32_t tmp, pllvco, pllr, pllsource, pllm;
+
+  /* Get SYSCLK source -------------------------------------------------------*/
+  switch (RCC->CFGR & RCC_CFGR_SWS)
+  {
+    case 0x04:  /* HSI used as system clock source */
+      SystemCoreClock = HSI_VALUE;
+      break;
+
+    case 0x08:  /* HSE used as system clock source */
+      SystemCoreClock = HSE_VALUE;
+      break;
+
+    case 0x0C:  /* PLL used as system clock  source */
+      /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN
+         SYSCLK = PLL_VCO / PLLR
+         */
+      pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC);
+      pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U ;
+      if (pllsource == 0x02UL) /* HSI used as PLL clock source */
+      {
+        pllvco = (HSI_VALUE / pllm);
+      }
+      else                   /* HSE used as PLL clock source */
+      {
+        pllvco = (HSE_VALUE / pllm);
+      }
+      pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 8);
+      pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> 25) + 1U) * 2U;
+      SystemCoreClock = pllvco/pllr;
+      break;
+
+    default:
+      break;
+  }
+  /* Compute HCLK clock frequency --------------------------------------------*/
+  /* Get HCLK prescaler */
+  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
+  /* HCLK clock frequency */
+  SystemCoreClock >>= tmp;
+}
+
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+

system_stm32g4xx.h

@@ -0,0 +1,104 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32g4xx.h
+  * @author  MCD Application Team
+  * @brief   CMSIS Cortex-M4 Device System Source File for STM32G4xx devices.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2019 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32g4xx_system
+  * @{
+  */
+
+/**
+  * @brief Define to prevent recursive inclusion
+  */
+#ifndef __SYSTEM_STM32G4XX_H
+#define __SYSTEM_STM32G4XX_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/** @addtogroup STM32G4xx_System_Includes
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+
+/** @addtogroup STM32G4xx_System_Exported_Variables
+  * @{
+  */
+  /* The SystemCoreClock variable is updated in three ways:
+      1) by calling CMSIS function SystemCoreClockUpdate()
+      2) by calling HAL API function HAL_RCC_GetSysClockFreq()
+      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
+         Note: If you use this function to configure the system clock; then there
+               is no need to call the 2 first functions listed above, since SystemCoreClock
+               variable is updated automatically.
+  */
+extern uint32_t SystemCoreClock;            /*!< System Clock Frequency (Core Clock) */
+
+extern const uint8_t  AHBPrescTable[16];    /*!< AHB prescalers table values */
+extern const uint8_t  APBPrescTable[8];     /*!< APB prescalers table values */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Exported_Constants
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Exported_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32G4xx_System_Exported_Functions
+  * @{
+  */
+
+extern void SystemInit(void);
+extern void SystemCoreClockUpdate(void);
+/**
+  * @}
+  */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /*__SYSTEM_STM32G4XX_H */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */

utils/filt.cpp

@@ -0,0 +1,67 @@
+#include <math.h>
+
+#include "filt.h"
+
+float FilterGPS::Limit(float Value, float Min, float Max)
+{
+	if (Value > Max) return Max;
+	if (Value < Min) return Min;
+	return Value;
+}
+
+float FilterGPS::Minimum(float Value1, float Value2)
+{
+	if (Value1 < Value2) return Value1;
+	return Value2;
+}
+
+void FilterGPS::Update(bool valid, float gps, float acc)
+{
+	const float ga = 9.80665f;
+
+	Delay.Acc[Delay.Index] = acc;
+	Delay.Pos[Delay.Index] = Position.Value;
+	Delay.Spd[Delay.Index++] = Speed.Value;
+	if (Delay.Index >= Delay.Size) Delay.Index = 0;
+
+	float prev = Delay.Acc[Delay.Index];
+	float shift = gps - Delay.Pos[Delay.Index];
+
+	float coef_a = 1;
+	if (Position.EnAcc) coef_a = Position.Acc / fabsf(prev);
+	float coef_p = 1;
+	if (Position.EnGPS) coef_p = powf(fabsf(shift * Position.Length), Position.Width);
+
+	float coef1 = Minimum(coef_a, coef_p);
+	coef1 = Limit(coef1, Position.Min, Position.Max);
+
+	Speed.Count++;
+	if (valid)
+	{
+		Speed.ValueGPS = (gps - Speed.Last_GPS) * Freq / Speed.Count;
+		Speed.Last_GPS = gps;
+		Speed.ValueAccGPS= (Speed.ValueGPS- Speed.Speed_GPS) * Freq / Speed.Count;
+		Speed.Speed_GPS = Speed.ValueGPS;
+		Speed.Count = 0;
+	}
+
+	for (int a = 0; a < Delay.Size; a++) Delay.Pos[a] += shift * coef1;
+	Position.Value += shift * coef1 + (Speed.Value / Freq) * (1 - coef1);
+
+	// SPEED
+
+	shift = Speed.ValueGPS - Delay.Spd[Delay.Index];
+
+	coef_a = 1;
+	if (Speed.EnAcc) coef_a = Speed.Acc / fabsf(prev);
+	float coef_g = 1;
+	if (Speed.EnAcc) coef_g = Speed.AccGPS / fabsf(Speed.ValueAccGPS);
+	float coef_s = 1;
+	if (Speed.EnGPS) coef_s = powf(fabsf(shift * Speed.Length), Speed.Width);
+
+	float coef2 = Minimum(coef_a, Minimum(coef_s, coef_g));
+	coef2 = Limit(coef2, Speed.Min, Speed.Max);
+
+	for (int a = 0; a < Delay.Size; a++) Delay.Spd[a] += shift * coef2;
+	Speed.Value += shift * coef2 + (acc * ga / Freq) * (1 - coef2);
+}

utils/filt.h

@@ -0,0 +1,56 @@
+#pragma once
+class FilterGPS
+{
+  public:
+
+    float Freq=100;
+
+    struct
+    {
+      float Value=0;
+      //---
+      float Min=0.0001, Max=0.1;
+      //---
+      bool EnAcc=true;
+      float Acc=0.02;
+      //---
+      bool EnGPS=true;
+      float Width=2, Length=1;
+      //---
+    }Position;
+
+    struct
+    {
+      float ValueAccGPS=0;
+      float ValueGPS=0;
+      float Value=0;
+      //---
+      float Min=0.0001, Max=0.1;
+      //---
+      bool EnAcc=true;
+      float Acc=0.02;
+      float AccGPS=0.02;
+      //---
+      bool EnGPS=true;
+      float Width=2, Length=1;
+      //---
+      float Last_GPS=0, Speed_GPS=0;
+      long Count=0;
+    }Speed;
+
+    struct
+    {
+      float Pos[50];
+      float Acc[50];
+      float Spd[50];
+      long Size=30, Index=0;
+    }Delay;
+
+    void Update(bool valid, float gps, float acc);
+  
+  private:
+    
+    float Limit(float Value, float Min, float Max);
+    float Minimum(float Value1, float Value2);
+
+};

utils/med.cpp

@@ -0,0 +1,72 @@
+#include <string.h>
+
+#include "med.h"
+
+long MED_Update(long Value, MED_Data16& Data)
+{
+  Data.Buf[Data.Index++] = Value;
+  if (Data.Index >= Data.Size) Data.Index = 0;
+
+  memcpy(Data.Temp, Data.Buf, Data.Size * sizeof(short));
+
+  short* t = Data.Temp;
+  long s = Data.Size;
+
+  for (long a = 0; a < s; a++)
+  for (long b = a; b < s; b++)
+  {
+    if (t[a] <= t[b]) continue;
+    short v = t[a];
+    t[a] = t[b];
+    t[b] = v;
+  }
+
+  return t[s / 2];
+}
+//------------------------------------------------------------------------------
+
+long MED_Update(long Value, MED_Data32& Data)
+{
+  Data.Buf[Data.Index++] = Value;
+  if (Data.Index >= Data.Size) Data.Index = 0;
+
+  memcpy(Data.Temp, Data.Buf, Data.Size * sizeof(long));
+
+  long* t = Data.Temp;
+  long s = Data.Size;
+
+  for (long a = 0; a < s; a++)
+  for (long b = a; b < s; b++)
+  {
+    if (t[a] <= t[b]) continue;
+    long v = t[a];
+    t[a] = t[b];
+    t[b] = v;
+  }
+
+  return t[s / 2];
+}
+//------------------------------------------------------------------------------
+
+float MED_Update(float Value, MED_DataF32& Data)
+{
+  Data.Buf[Data.Index++] = Value;
+  if (Data.Index >= Data.Size) Data.Index = 0;
+
+  memcpy(Data.Temp, Data.Buf, Data.Size * sizeof(float));
+
+  float* t = Data.Temp;
+  long s = Data.Size;
+
+  for (long a = 0; a < s; a++)
+  for (long b = a; b < s; b++)
+  {
+    if (t[a] <= t[b]) continue;
+    float v = t[a];
+    t[a] = t[b];
+    t[b] = v;
+  }
+
+  return t[s / 2];
+}
+//------------------------------------------------------------------------------

utils/med.h

@@ -0,0 +1,23 @@
+#pragma once
+
+struct MED_Data16
+{
+  short Size, Index;
+  short *Buf, *Temp;
+};
+
+struct MED_Data32
+{
+  long Size, Index;
+  long *Buf, *Temp;
+};
+
+struct MED_DataF32
+{
+  long Size, Index;
+  float *Buf, *Temp;
+};
+
+long MED_Update(long Value, MED_Data16& Data);
+long MED_Update(long Value, MED_Data32& Data);
+float MED_Update(float Value, MED_DataF32& Data);

utils/med2.cpp

@@ -0,0 +1,58 @@
+#include <string.h>
+
+#include "med2.h"
+
+void MED2_Init(MED2_Data& Data)
+{
+  for (unsigned char a = 0; a < Data.Size; a++) { Data.Index[a] = a; Data.Buff[a] = 0; }
+  Data.Last = 0;
+}
+
+long MED2_Update(long Value, MED2_Data& Data)
+{
+  unsigned char* index=Data.Index;
+  unsigned char size=Data.Size;
+  unsigned char& last=Data.Last;
+  long* buff=Data.Buff;
+  //---
+  buff[last] = Value;
+  //---
+  unsigned char id;
+  for (id = 0; id < size; id++) if (index[id] == last) break;
+  //---
+  bool f = (!id || (Value > buff[index[id - 1]]));
+
+  if (f)
+  {
+    for (unsigned char a = id; a < size - 1; a++)
+    {
+      if (buff[index[a + 1]] >= Value) break;
+      unsigned char tmp = index[a];
+      index[a] = index[a + 1];
+      index[a + 1] = tmp;
+    }
+  }
+  else
+  {
+    for (unsigned char a = id; a > 0; a--)
+    {
+      if (buff[index[a - 1]] <= Value) break;
+      unsigned char tmp = index[a - 1];
+      index[a - 1] = index[a];
+      index[a] = tmp;
+    }
+  }
+
+  last++;
+  if (last >= size) last = 0;
+  
+  if(Data.Smooth>1)
+  {
+    long ret=0;
+    long s = Data.Smooth/2;
+    for(long a=-s; a<s; a++) ret+=buff[index[size/2+a]];
+    return ret/Data.Smooth;
+  }
+  
+  return buff[index[size/2]];
+}

utils/med2.h

@@ -0,0 +1,12 @@
+#pragma once
+
+struct MED2_Data
+{
+  unsigned char Size, Last;
+  unsigned char* Index;
+  long* Buff;
+  unsigned char Smooth;
+};
+
+void MED2_Init(MED2_Data& Data);
+long MED2_Update(long Value, MED2_Data& Data);

utils/mot.cpp

@@ -0,0 +1,48 @@
+#include "mot.h"
+
+#include "pwm.h"
+  const unsigned long count = 4;
+ short pwm[count] {0,0,0,0};
+
+void QUAD_Set(short throt, short x, short y, short z)
+{
+  //  3   2
+  //  4   1
+  const unsigned char ESC_UL = 2;  // CW  // M3
+  const unsigned char ESC_UR = 1;  // CCW // M2
+  const unsigned char ESC_DL = 3;  // CCW // M4
+  const unsigned char ESC_DR = 0;  // CW  // M1
+  
+  
+
+  
+    for(int a=0; a<count; a++) pwm[a] = 0;
+  
+  pwm[ESC_UL] += x; pwm[ESC_UR] += x;
+  pwm[ESC_DL] -= x; pwm[ESC_DR] -= x;
+                        
+  pwm[ESC_UL] += y; pwm[ESC_UR] -= y; 
+  pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
+  
+  pwm[ESC_UL] += z; pwm[ESC_UR] -= z;
+  pwm[ESC_DL] -= z; pwm[ESC_DR] += z;
+  
+  short min=500, max=-500;
+  for(int a=0; a<count; a++)
+  {
+    if(pwm[a]>500) pwm[a]=500;
+    else if(pwm[a]<-500) pwm[a]=-500;
+    //---
+    if(min>pwm[a]) min=pwm[a];
+    if(max<pwm[a]) max=pwm[a];
+  }
+  
+  short up=(throt+max)-1000;
+  short down=(throt+min);
+  if(up>0) throt-=up;
+  if(down<0) throt-=down;
+  
+  for(int a=0; a<count; a++) pwm[a] += throt;
+  
+  PWM_SetQuad(pwm, 0, 1000);
+}

utils/mot.h

@@ -0,0 +1,5 @@
+#pragma once
+
+void QUAD_Set(short throt, short x, short y, short z);
+
+

utils/move.cpp

@@ -0,0 +1,102 @@
+#include "move.h"
+#include <math.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;
+
+Vec3 Motion(Vec3 vec, bool move, float dx, float dy, float range, unsigned long time)
+{
+  //static float move_x=0, move_y=0;
+  
+  //static float move_tof_x=0, move_tof_y=0;
+  //static float move_flo_x=0, move_flo_y=0;
+  
+  /*static long timer=0;
+  timer++;
+  if(timer>1000)
+  {
+    move_flo_x=move_flo_y=move_tof_x=move_tof_y=0;
+    move_x=0; move_y=0;
+    timer=0;
+  }*/
+  
+  //if(!move) return {0,0,0};
+  
+  static float len, high;
+  high = range * vec.z;               // cos(Z)
+  len = range * sqrtf(1-vec.z*vec.z); // sin(Z)
+  
+  static float a_x=0, a_y=0;
+  a_x=0, a_y=0;
+  
+  static float v_x, v_y;
+  
+  static float angle_shift=600;
+  
+  if(move)
+  {
+    a_x = (v_x-vec.x)*angle_shift;
+    a_y = (v_y-vec.y)*angle_shift;
+  }
+  
+  v_x=vec.x;
+  v_y=vec.y;
+  
+  static Vec3 v;
+  
+  float x=(dx+a_x), y=(dy-a_y);
+  
+  static float fx=0, fy=0;
+  
+  //fx=(fx*3+x)/4;
+  //fy=(fy*3+y)/4;
+  
+  return {x, y, high};
+  
+  //v = RotateToZ({dx-a_x, dy-a_y, 0});
+  
+  //v = RotateToZ({(dx+a_x)*(range/1000.0f), (dy-a_y)*(range/1000.0f), 0}, true);
+  
+  //v = RotateToZ({1000, 0, 0}, true);
+  //v = RotateToZ({(dx+a_x)*(range/1000.0f), 0, 0}, true);
+  
+  //v={dx+a_x, dy-a_y, 0};
+  
+  //move_x-=v.x;
+  //move_y-=v.y;
+  
+  
+  //move_flo_x+=dx;
+  //move_flo_y+=dy;
+  
+  //move_tof_x+=(float)a_x;
+  //move_tof_y+=(float)a_y;
+  
+  return v;
+  
+}
+
+/*void Motion(Vec3 vec, short dx, short dy, float range, unsigned long time)
+{
+  static long move_x=0, move_y=0;
+  
+  move_x+=dx;
+  move_y+=dy;
+  
+  static float len, high;
+  high = range * vec.z;               // cos(Z)
+  len = range * sqrtf(1-vec.z*vec.z); // sin(Z)
+  
+  static float a_x, a_y;
+  
+  a_x = range * vec.x;
+  a_y = -range * vec.y;
+  
+  static float m_x, m_y;
+  
+  m_x=move_x+a_x;
+  m_y=move_y+a_y;
+}*/

utils/move.h

@@ -0,0 +1,5 @@
+#pragma once
+
+#include "quat.h"
+
+Vec3 Motion(Vec3 vec, bool move, float dx, float dy, float range, unsigned long time);

utils/pid.cpp

@@ -0,0 +1,28 @@
+#include "pid.h"
+
+static inline float Saturation(float val, float min, float max)
+{
+  if (val < min) val = min;
+  if (val > max) val = max;
+  return val;
+}
+
+float PID_Update(float Value, float Current, PID_Data& Coef, float de)
+{
+  float p, i, d;
+
+  float e = Value - Current;
+
+  p = e * Coef.P.C;
+  p = Saturation(p, Coef.P.Min, Coef.P.Max);
+
+  i = Coef.i;
+  i += e * Coef.I.C;
+  i = Saturation(i, Coef.I.Min, Coef.I.Max);
+  Coef.i = i;
+  
+  d = de * Coef.D.C;
+  d = Saturation(d, Coef.D.Min, Coef.D.Max);
+
+  return Saturation(p + i + d, Coef.Min, Coef.Max);
+}

utils/pid.h

@@ -0,0 +1,14 @@
+#pragma once
+
+struct PID_Data
+{
+  float Min, Max;
+  struct
+  {
+    float Min, Max;
+    float C;
+  }P,I,D;
+  float i = 0;
+};
+
+float PID_Update(float Value, float Current, PID_Data& Coef, float de);

utils/prot.cpp

@@ -0,0 +1,593 @@
+#include <string.h>
+
+#include "tick.h"
+
+#include "ori.h"
+
+#include "uart.h"
+#include "gps.h"
+
+#include "pwm.h"
+
+#include "quat.h"
+
+#include "prot.h"
+#include "prot_head.h"
+
+Point PointGPS{0,0,0};
+
+MAIN_FLY_MODE MainFlyMode=MAIN_FLY_MODE::WAIT;
+
+STATUS_MODE Main_StatusMode;
+SYS_MODE Main_SysMode;
+
+const unsigned char SRC_ID = 1;
+const unsigned char DST_ID = 254;
+
+bool HeaderBegin::CheckCRC()
+{
+  unsigned char test = 0;
+  for (unsigned short a = 0; a < len1; a++) test ^= data[a];
+  return crc == test;
+}
+
+void HeaderBegin::SetCRC()
+{
+  unsigned char test = 0;
+  for (unsigned short a = 0; a < len1; a++) test ^= data[a];
+  crc = test;
+}
+
+enum class DataMode : unsigned char { Begin, Head, Data };
+
+static unsigned char ProtoData[MaxLength];
+static DataMode Mode = DataMode::Begin;
+static unsigned long Length, Index;
+
+static const unsigned long Wait = 100; // ms
+static unsigned long Timer;
+
+static void ProcessingMessage(HeaderMessages*);
+
+bool MainUpdate(unsigned char byte)
+{
+  const unsigned long tick = TICK_GetCount();
+
+  //if ((tick - Timer > 100)) // ?????? ?????? ?????????
+  //{
+  //  printf("------------> Warning: %d\n", tick - Timer);
+  //}
+
+  if ((Mode != DataMode::Begin) && (tick - Timer > Wait))
+  {
+    Mode = DataMode::Begin;
+    //printf("Error: timeout\n");
+  }
+
+  Timer = tick;
+
+  switch (Mode)
+  {
+    case DataMode::Begin:
+    {
+      if (byte != Global_stx) return true;
+      
+      Index = 0;
+      ProtoData[Index++] = byte;
+      Length = sizeof(HeaderBegin);
+      Mode = DataMode::Head;
+      
+      return true;
+    }
+
+    case DataMode::Head:
+    {
+      ProtoData[Index++] = byte;
+      if (Index < Length) return true;
+
+      HeaderBegin& begin = *(HeaderBegin*)ProtoData;
+
+      if ((begin.len1 != begin.len2) || (begin.len1 > MaxLength)
+          || (begin.len1 < sizeof(HeaderMessages)) || Index < sizeof(HeaderBegin))
+      {
+        //printf("Error: HeaderBegin -> size=%d\n", begin.len1);
+        Mode = DataMode::Begin;
+        return true;
+      }
+
+      Mode = DataMode::Data;
+      Length += begin.len1;
+
+      return true;
+    }
+
+    case DataMode::Data:
+    {
+      ProtoData[Index++] = byte;
+      if (Index < Length) return true;
+
+      HeaderBegin& begin = *(HeaderBegin*)ProtoData;
+      Mode = DataMode::Begin;
+      if (!begin.CheckCRC())
+      {
+        //printf("Error: HeaderBegin -> CRC\n");
+        return true;
+      }
+      ProcessingMessage((HeaderMessages*)(ProtoData + sizeof(HeaderBegin)));
+      return true;
+    }
+  }
+
+  return false;
+}
+
+void SendMes(HeaderBegin* begin)
+{
+    begin->SetCRC();
+    //SendData(begin, sizeof(HeaderBegin) + begin->len1);
+    
+    UART2_Send(begin, sizeof(HeaderBegin) + begin->len1);
+}
+//??????? ???????? ????????? ?????????
+
+static unsigned char MessageData[MaxLength];
+
+void* CreateMessageHeaders(HeaderBegin*& begin, unsigned long size, MESSAGES_ID msg_id)
+{
+    
+    begin = (HeaderBegin*)MessageData;
+    begin->len1 = begin->len2 = sizeof(HeaderMessages) + size;
+    begin->stx = Global_stx;
+    HeaderMessages& message = *(HeaderMessages*)begin->data;
+    message.msgId = msg_id;
+    message.srcId = SRC_ID;
+    message.dstId = DST_ID;
+    message.len = size;
+    message.timeusec = TICK_GetCount();
+    
+    return message.data;
+}
+//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? BatteryInfo
+void BatteryInfo()
+{
+  //????? ???????? ? ??????
+  HeaderBegin* begin;
+  PayloadBatteryInfo& info = *(PayloadBatteryInfo*) CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::BatteryInfo);
+  
+  info.perCharge = 95;
+  info.voltage = 14.5f;
+  info.amperage = 20.8f;
+  info.temp = 36.6f;
+  info.totalPower = 100.0f;
+  info.timeRemaining = 3600;
+  //printf("MessageTelemetry: BatteryInfo -> Send\n");
+  SendMes(begin);
+}
+
+NAV_SYS_MODE PROT_Nav=NAV_SYS_MODE::gps;
+
+//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? SysInfo
+void SysInfo()
+{
+    //????? ???????? ? ??????
+    unsigned int count_engine = 4;
+    HeaderBegin* begin;
+    PayloadSysInfo& info = *(PayloadSysInfo*)CreateMessageHeaders(begin, sizeof(PayloadSysInfo) + count_engine * sizeof(EnginePowerInfo), MESSAGES_ID::SysInfo);
+
+    info.mode = Main_SysMode;   //  !!!!!!!!!!!!
+    info.navSys = PROT_Nav;
+    info.pressureBaro = DataORI.Bar.Bar;
+    info.statusMode = Main_StatusMode;   // !!!!!!!!!!
+    info.engineStatus = PWM_Enable ? ENGINE_STATUS::enable : ENGINE_STATUS::disable;   //  !!!!!!!!!!!
+    info.engineCount = count_engine;
+    info.tempBaro = DataORI.Temp.Bar;
+
+    for (int i = 0; i < info.engineCount; i++)
+    {
+      EnginePowerInfo& pow = info.enginePower[i];
+      pow.power = 50;
+      pow.current = 1.0;
+      pow.engineSpeed = 100;
+      pow.voltage = 12.5;
+      pow.temp = 34.5;
+    }
+    //printf("MessageTelemetry: SysInfo -> Send\n");
+    
+    SendMes(begin);
+}
+//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? GyroInfo
+void GyroInfo()
+{
+    //????? ???????? ? ??????
+    HeaderBegin* begin;
+    PayloadGyroInfo& info = *(PayloadGyroInfo*)CreateMessageHeaders(begin, sizeof(PayloadGyroInfo), MESSAGES_ID::GyroInfo);
+
+    info.pitchGyroVel = DataORI.Gyr.X;
+    info.rollGyroVel = DataORI.Gyr.Y;
+    info.yawGyroVel = DataORI.Gyr.Z;
+    //printf("MessageTelemetry: GyroInfo -> Send\n");
+    SendMes(begin);
+}
+//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? AccelInfo
+void AccelInfo()
+{
+    //????? ???????? ? ??????
+    HeaderBegin* begin;
+    PayloadAccelInfo& info = *(PayloadAccelInfo*)CreateMessageHeaders(begin, sizeof(PayloadAccelInfo), MESSAGES_ID::AccelInfo);
+
+    info.aX = DataORI.Acc.X;
+    info.aY = DataORI.Acc.Y;
+    info.aZ = DataORI.Acc.Z;
+    info.pitchAccelVel = 0.0;
+    info.rollAccelVel = 0.0;
+    info.yawAccelVel = 0.0;
+    info.tempAccel = DataORI.Temp.Acc;
+    //printf("MessageTelemetry: AccelInfo -> Send\n");
+    SendMes(begin);
+}
+
+static GPS_BaseInfo Main_GPS;
+
+//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? GpsInfo
+void GpsInfo()
+{
+    
+    GPS_GetBaseInfo(Main_GPS);
+  
+    //????? ???????? ? ??????
+    HeaderBegin* begin;
+    PayloadGpsInfo& info = *(PayloadGpsInfo*)CreateMessageHeaders(begin, sizeof(PayloadGpsInfo), MESSAGES_ID::GpsInfo);
+
+    info.lat = Main_GPS.lat;
+    info.lon = Main_GPS.lon;
+    info.absAlt = Main_GPS.absAlt;
+    info.realAlt = Main_GPS.realAlt;
+    info.fixType = Main_GPS.fixType;
+    info.hdop = Main_GPS.hdop;
+    info.pdop = Main_GPS.pdop;
+    info.vdop = Main_GPS.vdop;
+    info.satUsed = Main_GPS.satUsed;
+    info.satVisible = Main_GPS.satVisible;
+    info.noise = Main_GPS.noise;
+    info.jamming = 0.0;
+    info.speed = Main_GPS.speed;
+    info.timeUTC = Main_GPS.timeUTC;
+    //printf("MessageTelemetry: GpsInfo -> Send\n");
+    SendMes(begin);
+}
+
+extern Vec3 test_iner;
+
+//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? InertialInfo
+void InertialInfo()
+{
+    //????? ???????? ? ??????
+    HeaderBegin* begin;
+    PayloadInertialInfo& info = *(PayloadInertialInfo*)CreateMessageHeaders(begin, sizeof(PayloadInertialInfo), MESSAGES_ID::InertialInfo);
+
+    info.x = test_iner.x;
+    info.y = test_iner.y;
+    info.z = test_iner.z;
+    info.baroAlt = DataORI.Bar.Alt;
+    info.pitch = DataORI.Pitch;
+    info.yaw = DataORI.Yaw;
+    info.roll = DataORI.Roll;
+    info.headingDeg = DataORI.Yaw;
+    info.speed = Main_GPS.speed;
+    info.pitchVel = 1.0;
+    info.yawVel = 1.0;
+    info.rollVel = 1.0;
+    //printf("MessageTelemetry: InertialInfo -> Send\n");
+    SendMes(begin);
+}
+//????????? ????? ?? ??????? ?? ???????? ???????? ??????
+void CommandAck(PayloadCommandAck& ack)
+{
+    HeaderBegin* begin;
+    PayloadCommandAck& info = *(PayloadCommandAck*)CreateMessageHeaders(begin, sizeof(PayloadCommandAck), MESSAGES_ID::Ack);
+    info = ack;
+    //printf("MessageCommandACK: Send\n");
+    SendMes(begin);
+}
+//?????????? ???????? ???????????
+static void ProcessingCommandLand()
+{
+    MainFlyMode=MAIN_FLY_MODE::STOP;
+    
+    // ????????? ???????...
+    PayloadCommandAck info;
+    info.commandId = COMMANDS_NAME::Land;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    info.status = FEASIBILITY_CODE_COMMAND::accepted;
+    CommandAck(info);
+
+}
+//?????????? ???????? ????????? ?????? (?????????)
+static void ProcessingCommandPause()
+{
+    // ????????? ???????...
+    PayloadCommandAck info;
+    info.commandId = COMMANDS_NAME::Pause;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    info.status = FEASIBILITY_CODE_COMMAND::accepted;
+    CommandAck(info);
+
+}
+//?????????? ???????? ???????? ? ????? ??????
+static void ProcessingCommandGoHome()
+{
+    // ????????? ???????...
+    PayloadCommandAck info;
+    info.commandId = COMMANDS_NAME::GoHome;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    info.status = FEASIBILITY_CODE_COMMAND::accepted;
+    CommandAck(info);
+
+}
+//?????????? ???????? ??????????? ??????
+static void ProcessingCommandContinue()
+{
+    // ????????? ???????...
+    PayloadCommandAck info;
+    info.commandId = COMMANDS_NAME::Continue;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    info.status = FEASIBILITY_CODE_COMMAND::accepted;
+    CommandAck(info);
+
+}
+//?????????? ???????? ??????? ???????
+static void ProcessingCommandStartEngine()
+{
+    if(MainFlyMode!=MAIN_FLY_MODE::STOP) MainFlyMode=MAIN_FLY_MODE::START;
+  
+    // ????????? ???????...
+    PayloadCommandAck info;
+    info.commandId = COMMANDS_NAME::StartEngine;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    info.status = FEASIBILITY_CODE_COMMAND::accepted;
+    CommandAck(info);
+
+}
+//?????????? ???????? ????????? ???????
+static void ProcessingCommandStopEngine()
+{
+    MainFlyMode=MAIN_FLY_MODE::WAIT;
+  
+    // ????????? ???????...
+    PayloadCommandAck info;
+    info.commandId = COMMANDS_NAME::StopEngine;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    info.status = FEASIBILITY_CODE_COMMAND::accepted;
+    CommandAck(info);
+
+}
+//?????????? ???????? ????????? ???????? ??????
+static void ProcessingCommandChangeSpeed(PayloadCommandChangeSpeed* payload)
+{
+    // ????????? ???????...
+    //payload->speed;
+
+    PayloadCommandAck info;
+    info.commandId = COMMANDS_NAME::ChangeSpeed;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    info.status = FEASIBILITY_CODE_COMMAND::accepted;
+    CommandAck(info);
+
+}
+//?????????? ???????? ????? ????????????? ???????
+static void ProcessingCommandChangeNav(PayloadCommandChangeNav* payload)
+{
+    PROT_Nav=(NAV_SYS_MODE)payload->nav_id;
+    // ????????? ???????...
+    //payload->nav_id;
+    PayloadCommandAck info;
+    info.commandId = COMMANDS_NAME::ChangeNav;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    info.status = FEASIBILITY_CODE_COMMAND::accepted;
+    CommandAck(info);
+}
+//?????????? ???????? ??????????? ? ????? ?? ?????????? ???????????
+static void ProcessingCommandGoToGlobal(PayloadCommandGoToGlobal* payload)
+{
+    PointGPS.Latitude=payload->lat;
+    PointGPS.Longitude=payload->lon;
+    PointGPS.Altitude=payload->abs_alt;
+   
+    if(MainFlyMode!=MAIN_FLY_MODE::STOP) MainFlyMode=MAIN_FLY_MODE::FLY;
+  
+    // ????????? ???????...
+    PayloadCommandAck info;
+    info.commandId = COMMANDS_NAME::GoToGlobal;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    info.status = FEASIBILITY_CODE_COMMAND::accepted;
+    CommandAck(info);
+
+}
+//?????????? ???????? ??????????? ? ????? ?? ????????? ???????????
+static void ProcessingCommandGoToLocal(PayloadCommandGoToLocal* payload)
+{
+    // ????????? ???????...
+    PayloadCommandAck info;
+    info.commandId = COMMANDS_NAME::GoToLocal;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    info.status = FEASIBILITY_CODE_COMMAND::accepted;
+    CommandAck(info);
+
+}
+
+//
+static void ProcessingCommandSetParameter(PayloadCommandSetParameter* payload)
+{
+    unsigned char cat_id = payload->cat_id; // ID ?????????
+    unsigned char param_id = payload->param_id; // ID ?????????
+    DATA_TYPES param_type = payload->param_type_code; // ??? ??????
+    unsigned char param_len = payload->param_len; // ?????? ??????? ??????
+    //payload->param_data; // ?????? ???? ?????????
+
+    PayloadCommandAck info;
+    info.commandId = COMMANDS_NAME::SetParameter;
+    info.status = FEASIBILITY_CODE_COMMAND::accepted;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    CommandAck(info);
+}
+
+//????? ?????????? ????????? ? ????????
+static void ProcessingCommand(CommandObj* commandObject) {
+    switch (commandObject->commandId)
+    {
+        case COMMANDS_NAME::Land: ProcessingCommandLand(); return;
+        case COMMANDS_NAME::Pause: ProcessingCommandPause(); return;
+        case COMMANDS_NAME::GoHome: ProcessingCommandGoHome(); return;
+        case COMMANDS_NAME::Continue: ProcessingCommandContinue(); return;
+        case COMMANDS_NAME::StartEngine: ProcessingCommandStartEngine(); return;
+        case COMMANDS_NAME::StopEngine: ProcessingCommandStopEngine(); return;
+        case COMMANDS_NAME::ChangeSpeed: ProcessingCommandChangeSpeed((PayloadCommandChangeSpeed*)commandObject->data); return;
+        case COMMANDS_NAME::ChangeNav: ProcessingCommandChangeNav((PayloadCommandChangeNav*)commandObject->data); return;
+        case COMMANDS_NAME::GoToGlobal: ProcessingCommandGoToGlobal((PayloadCommandGoToGlobal*)commandObject->data); return;
+        case COMMANDS_NAME::GoToLocal: ProcessingCommandGoToLocal((PayloadCommandGoToLocal*)commandObject->data); return;
+        case COMMANDS_NAME::SetParameter: ProcessingCommandSetParameter((PayloadCommandSetParameter*)commandObject->data); return;
+    }
+}
+//?????????? ????????? ???????????? ?????
+static void ConnectionTest()
+{
+    HeaderBegin* begin;
+    CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::ConnectionTest);
+
+    begin->SetCRC();
+
+    UART2_Send(begin, sizeof(HeaderBegin) + begin->len1);
+}
+//?????????? ????????? ??????? ??????? ???????
+static void StatusCommand(PayloadStatusCommand* payload) 
+{
+    // payload->commandId; ??? ?????? ???? ?????-?? ???????? ???????? ????????? ?????????? ??????? ?? ?? ID
+    HeaderBegin* begin;
+    PayloadStatusCommand& info = *(PayloadStatusCommand*)CreateMessageHeaders(begin, sizeof(PayloadStatusCommand), MESSAGES_ID::StatusCommand);
+    info.commandId = payload->commandId;
+    info.executionCode = EXEC_CODE_COMMAND::completed;
+    info.errorCode = ERROR_CODE_COMMAND::No_error;
+    SendMes(begin);
+}
+
+static void CountMenuCategories() 
+{
+    //
+    unsigned char count_categories = 4;
+    unsigned char categoriesNamesLen[4] = { 5, 6, 5, 7 };
+    HeaderBegin* begin;
+    PayloadCountMenuCategories& info = *(PayloadCountMenuCategories*)CreateMessageHeaders(begin, sizeof(PayloadCountMenuCategories) + count_categories * sizeof(unsigned char), MESSAGES_ID::CountMenuCategories);
+
+    info.countCategories = count_categories;
+    for (int i = 0; i < info.countCategories; i++) {
+        info.data[i] = categoriesNamesLen[i];
+    }
+    //printf("MessageTelemetry: SysInfo -> Send\n");
+    SendMes(begin);
+}
+
+static void MenuCategories(PayloadMenuCategories* payload)
+{
+    const char* names[4] = { "Main1","PID_S1","MOTOR", "ACCELER"}; // ????? ?????????
+    unsigned char categoriesNamesLen[4] = { 5, 6, 5, 7 }; // ????? ???? ????????? (????? ? ????????? utf-8)
+    unsigned char req_count = payload->countCategories;
+    unsigned char total_len_names = 0;
+    for (int i = 0; i < req_count; i++) 
+    {
+        unsigned char cat_id = (payload->categories)[i];
+        unsigned char cat_name_len = categoriesNamesLen[(payload->categories)[i]];
+        total_len_names += cat_name_len;
+    }
+    HeaderBegin* begin;
+    PayloadMenuCategories& info = *(PayloadMenuCategories*)CreateMessageHeaders(begin, sizeof(PayloadMenuCategories) + req_count * sizeof(MenuCategori) + total_len_names * sizeof(unsigned char), MESSAGES_ID::CategoriesMenu);
+    info.countCategories = req_count;
+    MenuCategori* menu = (MenuCategori*)info.categories;
+    for (int i = 0; i < req_count; i++) 
+    {
+        menu->id = (payload->categories)[i];
+        menu->count_param = 1;
+        menu->len_name = categoriesNamesLen[menu->id];
+        memcpy(menu->name, names[menu->id], menu->len_name);
+        menu = (MenuCategori*)(((char*)menu) + sizeof(MenuCategori) + menu->len_name);
+    }
+    SendMes(begin);
+}
+
+static void CategoriesParameters(PayloadCategoriesParameters* payload)
+{
+    unsigned char cat_id = payload->cat_id; // id ????????? ??? ?????? ?????????
+    unsigned char param_id = payload->param_id; // id ????????? ??? ?????? ?????????
+    // ????????? ?????????
+    const char* name = "PARAMETER1";
+    unsigned char len_name = 10;
+    float param1 = 0.231;
+    DATA_TYPES param_type = DATA_TYPES::dt_float_32;
+    //
+    
+    HeaderBegin* begin;
+    PayloadCategoriesParameters& info = *(PayloadCategoriesParameters*)CreateMessageHeaders(begin, sizeof(PayloadCategoriesParameters) + len_name + sizeof(param1), MESSAGES_ID::CategoriesParameters);
+    info.cat_id = cat_id;
+    info.param_id = param_id;
+    info.param_name_len = len_name;
+    info.param_len = sizeof(param1);
+    info.param_type_code = param_type;
+    memcpy(info.param_data, name, len_name);
+    memcpy(&info.param_data[len_name], &param1, sizeof(param1));
+    SendMes(begin);
+}
+
+//void Ping(HeaderMessages* message)
+//{
+//  HeaderBegin* begin;
+//  CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::Ping);
+//  //---
+//  SendMes(begin);
+//  //---
+//  printf("Ping %d Pong %d \n", message->timeusec, ((HeaderMessages*)begin->data)->timeusec);
+//}
+//
+//void Pong(HeaderMessages* message)
+//{
+//  HeaderBegin* begin;
+//  CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::Pong);
+//  //---
+//  SendMes(begin);
+//  //---
+//  printf("Pong %d Ping %d \n", message->timeusec, ((HeaderMessages*)begin->data)->timeusec);
+//}
+
+static void ProcessingMessage(HeaderMessages* message)
+{
+  switch (message->msgId)
+  {
+    /*case MESSAGES_ID::Ping: Pong(message); return;
+    case MESSAGES_ID::Pong: Ping(message); return;*/
+    case MESSAGES_ID::BatteryInfo: BatteryInfo(); return;
+    case MESSAGES_ID::GyroInfo: GyroInfo(); return;
+    case MESSAGES_ID::AccelInfo: AccelInfo(); return;
+    case MESSAGES_ID::GpsInfo: GpsInfo(); return;
+    case MESSAGES_ID::InertialInfo: InertialInfo(); return;
+    case MESSAGES_ID::SysInfo: SysInfo(); return;
+    case MESSAGES_ID::Command: ProcessingCommand((CommandObj*)message->data); return;
+    case MESSAGES_ID::ConnectionTest: ConnectionTest(); return;
+    case MESSAGES_ID::StatusCommand: StatusCommand((PayloadStatusCommand*)message->data); return;
+    
+    case MESSAGES_ID::CountMenuCategories: CountMenuCategories(); return;
+    case MESSAGES_ID::CategoriesMenu: MenuCategories((PayloadMenuCategories*)message->data); return;
+    case MESSAGES_ID::CategoriesParameters: CategoriesParameters((PayloadCategoriesParameters*)message->data); return;
+  }
+}
+
+//void TestPing()
+//{
+//  HeaderMessages message;
+//  message.timeusec = GetTickCount();
+//  Pong(&message);
+//}
+
+void PROT_Update()
+{
+  char byte;
+  unsigned char size = UART2_Recv(&byte, 1);
+  if (size) MainUpdate(byte);
+}

utils/prot.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "prot_head.h"
+
+extern Point PointGPS;
+
+enum class MAIN_FLY_MODE { WAIT, START, FLY, STOP };
+
+extern MAIN_FLY_MODE MainFlyMode;
+
+extern STATUS_MODE Main_StatusMode;
+extern SYS_MODE Main_SysMode;
+
+void PROT_Update();

utils/prot_head.h

@@ -0,0 +1,318 @@
+#pragma once
+
+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,
+
+	//Ping = 200, // :)
+	//Pong = 201, // :]
+};
+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
+{
+  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
+};
+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
+};
+
+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
+};
+
+#pragma pack(push,1)
+
+const unsigned short MaxLength = 256;
+const unsigned char Global_stx = 0xAA;
+//?????? ?????????
+struct HeaderBegin
+{
+  unsigned char stx;
+  unsigned short len1; //Len header + payload (message) 
+  unsigned short len2; //Len header + payload (message)
+  unsigned char crc; // CRC MESSAGE
+  unsigned char data[0]; // Message
+  //---
+  bool CheckCRC();
+  void SetCRC();
+};
+// ????????? ?????????
+struct HeaderMessages
+{
+  MESSAGES_ID msgId;
+  unsigned char srcId;
+  unsigned char dstId;
+  unsigned char len;
+  unsigned long long timeusec;
+  unsigned char data[0];
+};
+struct CommandObj
+{
+	COMMANDS_NAME commandId;
+	unsigned char data[0];
+};
+//???????? ???????? ????????? ? ??????? ?? ???
+struct PayloadBatteryInfo
+{
+  unsigned char perCharge;
+  float voltage;
+  float amperage;
+  float temp;
+  float totalPower;
+  unsigned long long timeRemaining;
+};
+// ????????? ? ??????? ?? ??????? ??????
+struct EnginePowerInfo
+{
+	unsigned char power; //??????? ???????? ?? ????? ?? 0 ?? 100%
+	unsigned int engineSpeed;//??????? ?????? ? ??????? ??????
+	float current; // ??? ???????????? ???????
+	float voltage; // ?????????? ?? ??????
+	float temp; // ??????????? ??????
+
+};
+struct PayloadSysInfo // ???????? ???????? ? ??????? ? ???????? ????
+{
+	SYS_MODE mode; //??????? ????? ?????? (????? - 1, ??????? - 2, ????? ??????????????? ???????? ????? - 3, ????? ??????????? ?????? - 4, ???????? - 5)
+	STATUS_MODE statusMode; //??????? ?????? ????? (?? ????? (on ground) - 1, ????? ?????? (TakingOff) - 2, ????? (In air) - 3, ??????????? (Landing) - 4)
+	NAV_SYS_MODE navSys; // ??????? ???????????? ??????? ????????? (GPS\???\Optical Flow) ??? - 1, GPS - 2, Optical Flow - 3
+	ENGINE_STATUS engineStatus; // ??????? ????????? ???????. ???? - 0, ??? - 1.
+	unsigned char engineCount; // ?????????? ?????????? ?? ????
+	float pressureBaro; //??????????? ????????
+	float tempBaro; // ??????????? ???. ?????
+	EnginePowerInfo enginePower[0];
+};
+struct PayloadGyroInfo // ???????? ???????? ? ??????? ?? ?????????
+{
+	float yawGyroVel; // ??????? ???????? ????????? ???? ???????? (? ??? ????? ??????????? ???\??)
+	float pitchGyroVel; // ??????? ???????? ????????? ???? ??????? (? ??? ????? ??????????? ???\??)
+	float rollGyroVel; // ??????? ???????? ????????? ???? ?????  (? ??? ????? ??????????? ???\??)
+};
+struct PayloadAccelInfo // ???????? ???????? ????????? ? ??????? ?? ?????????????
+{
+	float yawAccelVel; // ??????? ????????? ????????? ???? ???????? (Z)
+	float pitchAccelVel; // ??????? ????????? ????????? ???? ??????? (Y)
+	float rollAccelVel; // ??????? ????????? ????????? ???? ????? (X)
+	float aX; // ????????? ?? X (?\?^2)
+	float aY; // ????????? ?? ? (?\?^2)
+	float aZ; // ????????? ?? Z (?\?^2)
+	float tempAccel; //??????????? ?????????????
+};
+struct PayloadGpsInfo // ???????? ???????? ????????? ? ??????? ?? GPS ???????
+{
+	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; // UTC - GPS
+};
+struct PayloadInertialInfo // ???????? ???????? ? ??????? ?? ???????????? ??????? ?????????
+{
+	float x; // ?????????? X (?? ????????? ??????? ?????????)
+	float y; // ?????????? Y (?? ????????? ??????? ?????????)
+	float z; // ?????????? ?????? (?? ????????? ??????? ?????????)
+	float headingDeg; // ??????? ??????????? ???????? ???? ???????????? ??????
+	float speed; // ???????? ???? (?????????? ?? ?? GPS)
+	float roll; // ??????? ???? ????? ? ????????
+	float pitch; // ??????? ???? ??????? ? ????????
+	float yaw; // ??????? ???? ???????? ? ????????
+	float rollVel; // ???????? ?? ??????????? ????? ?\? (? ??????????? ??? Y)
+	float pitchVel; // ???????? ?? ??????????? ??????? ?\? (? ??????????? ??? ?)
+	float yawVel; // ???????? ?? ??????????? ???????? ?\? (? ??????????? ??? Z)
+	float baroAlt; // ?????? ? ?????? ?? ?????????
+};
+//???????? ???????? ????????? ? ??????? ? ????????? ???????
+struct PayloadCommandAck
+{
+	COMMANDS_NAME commandId;
+	FEASIBILITY_CODE_COMMAND status; // 0 - ?? ????? ???? ????????? (??????????? ??? ??????), 1 - ???????
+	ERROR_CODE_COMMAND errorCode;
+};
+struct PayloadStatusCommand
+{
+	COMMANDS_NAME commandId;
+	EXEC_CODE_COMMAND executionCode; // 0 - ?????? ??? ?????????? ,1 - ??????????? ,2 - ????????? ???????.
+	ERROR_CODE_COMMAND errorCode;
+};
+// ???????? ???????? ??????? ??? ????? ???????? ??????
+struct PayloadCommandChangeSpeed
+{
+	float speed;
+};
+// ???????? ???????? ??????? ??? ????? ??????? ???. ???. ???????.
+struct PayloadCommandChangeNav
+{
+	unsigned char nav_id; // Inertial - 1, GPS = 2, Oprical_Flow = 3
+};
+// ???????? ???????? ??????? ??? ??????????? ? ????? ?? ?????????? ???????????
+struct PayloadCommandGoToGlobal
+{
+	float lat;
+	float lon;
+	float abs_alt;
+	float speed;
+};
+// ???????? ???????? ??????? ??? ??????????? ? ????? ?? ????????? ???????????
+struct PayloadCommandGoToLocal
+{
+	float x;
+	float y;
+	float z;
+	float speed;
+};
+
+struct PayloadCountMenuCategories
+{
+	unsigned char countCategories;
+	unsigned char data[0];
+};
+
+struct MenuCategori
+{
+	unsigned char id;
+	unsigned char len_name;
+	unsigned char count_param;
+	char name[0];
+};
+
+struct PayloadMenuCategories
+{
+	unsigned char countCategories;
+	unsigned char categories[0];
+};
+
+struct PayloadCategoriesParameters
+{
+	unsigned char cat_id;
+	unsigned char param_id;
+	unsigned char param_name_len;
+	unsigned char param_len;
+	DATA_TYPES param_type_code;
+	char param_data[0];
+};
+
+struct PayloadCommandSetParameter
+{
+	unsigned char cat_id;
+	unsigned char param_id;
+	DATA_TYPES param_type_code;
+	unsigned char param_len;
+	char param_data[0];
+};
+
+#pragma pack(pop)

utils/quat.cpp

@@ -0,0 +1,275 @@
+#include "quat.h"
+#include <math.h>
+
+static const float PI = 3.14159265359f;
+static const float TO_DEG = 180.0f/PI;
+static const float TO_RAD = PI/180.0f;
+
+struct Quaternion 
+{
+  float w, x, y, z;
+};
+
+static Quaternion qCurrent = { 1, 0, 0, 0 };
+
+static const float period = 0.01f; // 100Hz
+
+static bool isFirst = true;
+
+inline void vecNormalize(Vec3& v)
+{
+  float n = sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
+  if (n > 1e-9f) 
+  {
+    v.x /= n;
+    v.y /= n;
+    v.z /= n;
+  }
+}
+
+inline Vec3 vecCross(const Vec3& a, const Vec3& b)
+{
+  return 
+  {
+    a.y * b.z - a.z * b.y,
+    a.z * b.x - a.x * b.z,
+    a.x * b.y - a.y * b.x
+  };
+}
+
+inline void normalizeQuaternion(Quaternion& q)
+{
+  float norm = sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
+  if (norm > 1e-12f) 
+  {
+    q.w /= norm;
+    q.x /= norm;
+    q.y /= norm;
+    q.z /= norm;
+  }
+}
+
+static Quaternion quaternionMultiply(const Quaternion& q1, const Quaternion& q2)
+{
+  Quaternion r;
+  r.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
+  r.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
+  r.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
+  r.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
+  return r;
+}
+
+static Quaternion rotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
+{
+  Quaternion r = quaternionMultiply(quaternionMultiply(q, Quaternion{ 0, In.x, In.y, In.z }), Quaternion{ q.w, -q.x, -q.y, -q.z });
+  
+  return r;
+}
+
+static Vec3 backRotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
+{
+  Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{q.w, -q.x, -q.y, -q.z}, Quaternion{ 0, In.x, In.y, In.z }), q);
+  
+  return { r.x, r.y, r.z };
+}
+
+static Quaternion backRotateVectorByQuaternion2(const Quaternion& q, const Quaternion& In)
+{
+  Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{q.w, -q.x, -q.y, -q.z}, In), q);
+  
+  return r;
+}
+
+static Quaternion createYawQuaternion(float angle)
+{
+  Quaternion q;
+  
+  q.w = cosf(angle);
+  q.x = 0.0f;
+  q.y = 0.0f;
+  q.z = sinf(angle);
+  
+  return q;
+}
+
+static Quaternion AccQuaternion(Quaternion& current, Vec3 a, float gravity, Vec3 xyz, float& error)
+{
+  float acc = sqrtf(a.x*a.x+a.y*a.y+a.z*a.z);
+  if(acc>(1.0f+gravity) || acc<(1.0f-gravity)) return current;
+  
+  vecNormalize(a);
+  
+  float x=a.x-xyz.x, y=a.y-xyz.y, z=a.z-xyz.z;
+  
+  error=sqrtf(x*x+y*y+z*z)/10.0f;
+  
+  Vec3 g { 0, 0, 1 };
+  Vec3 axis = vecCross(g, a);
+  float w = 1 + (g.x * a.x + g.y * a.y + g.z * a.z);
+  Quaternion q = { w, axis.x, axis.y, axis.z };
+  normalizeQuaternion(q);
+  
+  Quaternion qYaw {current.w, 0, 0, current.z};
+  
+  return quaternionMultiply(q, qYaw); // ??????????? ?????? ?? Z
+}
+
+static Quaternion GyroQuaternion(Quaternion& current, float wx, float wy, float wz)
+{
+  Quaternion Mapp = current;
+  Quaternion Spd { 0, wx, wy, wz };
+  Quaternion aq = quaternionMultiply(Spd, Mapp);
+  
+  Mapp.w -= 0.5f * aq.w;
+  Mapp.x -= 0.5f * aq.x;
+  Mapp.y -= 0.5f * aq.y;
+  Mapp.z -= 0.5f * aq.z;
+  
+  normalizeQuaternion(Mapp);
+  return Mapp;
+}
+
+static Quaternion nlerp(const Quaternion& q1, const Quaternion& q2, float alpha)
+{
+
+  float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
+  
+  Quaternion q2_ = q2;
+  if (dot < 0)
+  {
+    q2_.w = -q2.w;
+    q2_.x = -q2.x;
+    q2_.y = -q2.y;
+    q2_.z = -q2.z;
+  }
+  
+  Quaternion r;
+  r.w = (1.0f - alpha) * q1.w + alpha * q2_.w;
+  r.x = (1.0f - alpha) * q1.x + alpha * q2_.x;
+  r.y = (1.0f - alpha) * q1.y + alpha * q2_.y;
+  r.z = (1.0f - alpha) * q1.z + alpha * q2_.z;
+  
+  normalizeQuaternion(r);
+  
+  return r;
+}
+
+inline float GetAngle(float a1, float a2, float az)
+{
+  if(a2 == 0.0f && az == 0.0f) 
+  {
+    if(a1>0.0f) return 90.0f;
+    if(a1<0.0f) return -90.0f;
+    return 0.0f;
+  }
+  
+  return atanf(a1/sqrtf(a2*a2+az*az)) * TO_DEG;
+}
+
+static Vec3 quaternionToPitchRollYaw(const Quaternion& q, Vec3& cosXYZ)
+{
+  Quaternion pry=rotateVectorByQuaternion(q, {0, 0, 1});
+  
+  float yaw = 2.0f * atan2f(q.z, q.w) * TO_DEG;
+  if(yaw<0.0f) yaw=360.0f+yaw;
+  
+  cosXYZ = {pry.x, pry.y, pry.z};
+  
+  return // Sovereign orientation 
+  {
+    GetAngle(pry.y, pry.x, pry.z),  // Pitch
+    GetAngle(-pry.x, pry.y, pry.z), // Roll
+    yaw                             // Yaw
+  };
+}
+
+static void addMagneto(Quaternion& q, Vec3 mag, float alpha, const float shift)
+{
+  static Quaternion yq = createYawQuaternion(shift*TO_RAD);
+  
+  vecNormalize(mag);
+  
+  Quaternion mQ={0, mag.x, mag.y, mag.z};
+  
+  Quaternion mW = backRotateVectorByQuaternion2(q, mQ);
+  
+  mW=quaternionMultiply(mW, yq); // Shifting the axes to the true north
+  
+  float gamma = mW.x * mW.x + mW.y * mW.y;
+  float beta = sqrtf(gamma + mW.x * sqrtf(gamma));
+  
+  Quaternion mD
+  {
+    beta / (sqrtf(2.0f * gamma)),
+    0.0f,
+    0.0f,
+    mW.y / (sqrtf(2.0f) * beta),
+  };
+  
+  mD.w = (1.0f - alpha) + alpha * mD.w;
+  mD.z = alpha * mD.z;
+  
+  if(mD.w!=mD.w || mD.x!=mD.x || mD.y!=mD.y || mD.z!=mD.z) return;
+  
+  q=quaternionMultiply(q, mD);
+  normalizeQuaternion(q);
+}
+
+ORI WorkAccGyroMag(const Vec3 acc, const Vec3 gyr, const Vec3 mag, const float mag_shift, const float alpha)
+{
+    float wx = gyr.x * 500.0f / 30000.0f * TO_RAD * period;
+  float wy = gyr.y * 500.0f / 30000.0f * TO_RAD * period;
+  float wz = gyr.z * 500.0f / 30000.0f * TO_RAD * period;
+
+  Vec3 aB = acc;
+  
+  Vec3 cosXYZ;
+  Vec3 pry=quaternionToPitchRollYaw(qCurrent, cosXYZ);
+  
+  static float error=0;
+ // Quaternion qAcc = AccQuaternion(qCurrent, aB, 0.05f, cosXYZ, error); // Tolerance for gravity deviation 5%
+  
+  qCurrent = GyroQuaternion(qCurrent, wx, wy, wz);
+  
+  if(error>0.01f) error=0.01f;
+  if(error<alpha) error=alpha;
+  
+  /*
+  Quaternion qFused = nlerp(qCurrent, qAcc, error);
+  
+  if (isFirst)
+  {
+    qFused = qAcc;
+    isFirst = false;
+  }
+  
+  qCurrent = qFused;
+  */
+  //addMagneto(qCurrent, mag, alpha, mag_shift);
+  
+  Vec3 ine=backRotateVectorByQuaternion(qCurrent, aB);
+  
+  return
+  {
+    cosXYZ.x, cosXYZ.y, cosXYZ.z,
+    pry.x, pry.y, pry.z,
+    ine.x, ine.y, ine.z,
+  };
+}
+
+Vec3 RotateToZ(const Vec3 vec, bool Rev)
+{
+  Quaternion v { 0, vec.x, vec.y, vec.z };
+  Quaternion q = { qCurrent.w, 0, 0, Rev ? -qCurrent.z : qCurrent.z };
+  normalizeQuaternion(q);
+  
+  q = quaternionMultiply(quaternionMultiply(v, q), q); // ??????????? ?????? ?? Z
+  
+  return { q.x, q.y, q.z };
+}
+
+void GoZero()
+{
+  qCurrent= {1.0f, 0.0f, 0.0f, 0.0f};
+
+}

utils/quat.h

@@ -0,0 +1,15 @@
+#pragma once
+
+struct Vec3 { float x, y, z; };
+
+struct ORI
+{
+  float sinX, sinY, cosZ; // Earth's plane tilt
+  float Pitch, Roll, Yaw; // Sovereign orientation (not Euler)
+  float IneX, IneY, IneZ; // Inertial accelerations
+};
+
+ORI WorkAccGyroMag(const Vec3 acc, const Vec3 gyr, const Vec3 mag, const float mag_shift, const float alpha);
+
+Vec3 RotateToZ(const Vec3 vec, bool Rev=false);
+void GoZero();