Новый main

2026-04-16 16:48:40 +03:00
parent 273398ba16
commit a3d845df9e
9 changed files with 604 additions and 1 deletions
--- a/Source/main.cpp
+++ b/Source/main.cpp
@@ -1,12 +1,111 @@
 #include "stm32g4xx.h"
+#include "Tick.h"
+#include "Tim.h"
+#include "IIMU.h"
 #include "ICM20948.h"
+#include "IRS.h"
+#include "Vector.h"
+#include "Autopilot.h"

 extern "C" void SystemClock_Config();

+constexpr long TIM_PRIORITY = 2;
+
+IMU_Data DataIMU;
+
+IRS MainIRS;
+
+IIMU* IMUObj;
+
+float GetCurrentTime()
+{
+  return ((float)TICK_GetCount()) / 1000.0f;
+}
+
+void DoneProcIMU(IMU_Data& Data)
+{
+  DataIMU = Data;
+  // MagReady=true;
+}
+
+void TimerUpdateSensor()
+{
+  //IMUObj->GetAsync(DoneProcIMU);
+  if(MainDrone.IMUInit) IMUObj->GetAsync(DoneProcIMU);
+  //if(MainDrone.BarInit) BarObj->GetAsync(DoneProcBAR);
+}
+
+void TimerUpdateMain()
+{
+  float time = GetCurrentTime();
+  
+  Vector3 gyr = {(float)DataIMU.Gyr.X, (float)DataIMU.Gyr.Y, (float)DataIMU.Gyr.Z};
+  Vector3 acc = {(float)DataIMU.Acc.X, (float)DataIMU.Acc.Y, (float)DataIMU.Acc.Z};
+  
+  // Vector3 mag;
+  // if(MainDrone.ExternMagInit) mag = {(float)DataMAG.X, (float)DataMAG.Y, (float)DataMAG.Z};
+  // else mag = {(float)DataIMU.Mag.X, (float)DataIMU.Mag.Y, (float)DataIMU.Mag.Z};
+  
+  MainIRS.UpdateGyro(gyr);
+  MainIRS.UpdateAccel(acc);
+  /*if(MagReady)
+  {
+    MagReady=false;
+    MainIRS.UpdateMagnet(mag);
+  }
+  
+  MainGPS.UpdateAverage(time);
+  MainBar.UpdateAverage();
+  MainOF.UpdateAverage(IRS::Period);
+  MainLen.UpdateAverage();
+  
+  if(DataFLOW.Update)
+  {
+    DataFLOW.Update=false;
+    UpdateFlow(DataFLOW.OF, MainIRS.Inertial.Pos.Z - MainIRS.GroundShift, DataFLOW.Valid, DataFLOW.Quality);
+  }
+  
+  if(DataTOF.Update)
+  {
+    DataTOF.Update=false;
+    UpdateRange(DataTOF.Range, DataTOF.Valid && DataTOF.Range>0.02f && DataTOF.Strength>200.0f);
+  }
+  
+  UpdateBar(DataBAR.Pressure, DataBAR.Temp, DataBAR.Pressure>0.0f);
+  
+  if(DataGPS.Update)
+  {
+    DataGPS.Update=false;
+    Vector3 gps = { DataGPS.X, DataGPS.Y, DataGPS.Z };
+    UpdateGPS(gps, time, DataGPS.Valid);
+  }*/
+  
+  Vector3 pos;
+  MainIRS.RestoreAllShift(pos);
+  /*MainVel.PointBegin-=pos;
+  
+  AutoControl(gyr);
+  
+  ReadStateINS();
+  
+  if(MainDrone.ExternMagInit) MagObj->GetAsync(DoneProcMag);*/
+}
+
 int main()
 {
  SystemClock_Config(); // 170MHz
  IMU_InitPower();
  
-  return 0;
+  TICK_Init();
+  
+  TIM6_Init(TIM_PRIORITY, 500, TimerUpdateSensor, TimerUpdateMain);
+  
+  IMUObj = TryFindIMU(MainDrone.IMUInit);
+  
+  TIM6_Enable();
+  
+  while (true)
+  {
+    
+  }
 }