Реализован уровень обработки данных драйвера IMU

Source/Devices/IIMU.cpp

@@ -1,2 +1,43 @@
 #include "IIMU.h"
+#include "I2C.h"
+#include "ICM20948.h"
+
+IMU_Info IMUInfo;
+
+IMU_Calib_Data CalibDataIMU;
+
+void IMU_SetReg(unsigned char Addr, unsigned char Reg, unsigned char Value)
+{
+  unsigned char reg[2];
+  reg[0] = Reg; reg[1] = Value;
+  I2C1_Write(Addr, reg, 2);
+  I2C1_Stop();
+}
+
+unsigned char IMU_GetReg(unsigned char Addr, unsigned char Reg)
+{
+  I2C1_Write(Addr, Reg);
+  I2C1_Read(Addr, &Reg, 1);
+  I2C1_Stop();
+  return Reg;
+}
+
+IIMU* TryFindIMU(bool& find)
+{
+  I2C1_Init();
+  IIMU* Imu = nullptr;
+  for (int i = 0; i < sizeof(IMUInfo.Addrs); ++i)
+  {
+    find = I2C1_CheckDeviceWhoAmI(IMUInfo.Addrs[i], IMUInfo.WhoIAMReg[i], IMUInfo.ExpectedIDs[i]);
+    if (find && i == 0)
+    {
+      static ICM20948 imuICM20948;
+      Imu = (IIMU*)&imuICM20948;
+      break;
+    }
+    for (int a = 0; a < 10000000; ++a) { asm volatile("NOP"); }
+  }
+  if (find) Imu->Init();
+  return Imu;
+}