Полный переход на C++

*Чтение IMU и обработка его данных выполняется в точности как в рабочей прошивке.
*Определение вращения работает корректно.

Source/main.cpp

@@ -6,6 +6,11 @@
 #include "IRS.h"
 #include "Vector.h"
 #include "Autopilot.h"
+#include "RadioReceiver.h"
+#include "Motors.h"
+#include "Attitude.h"
+
+#define PI            3.14159265359f
 
 extern "C" void SystemClock_Config();
 
@@ -22,6 +27,23 @@ float GetCurrentTime()
   return ((float)TICK_GetCount()) / 1000.0f;
 }
 
+Vector3 QuatToEuler(const Quaternion& q)
+{
+  Vector3 e;
+  
+  e.X = atan2f(2*(q.W*q.X + q.Y*q.Z), 1 - 2*(q.X*q.X + q.Y*q.Y));
+
+  e.Y = asinf(2*(q.W*q.Y - q.Z*q.X));
+
+  e.Z = atan2f(2*(q.W*q.Z + q.X*q.Y), 1 - 2*(q.Y*q.Y + q.Z*q.Z));
+
+  e.X *= 180.0f / PI;
+  e.Y *= 180.0f / PI;
+  e.Z *= 180.0f / PI;
+
+  return e;
+}
+
 void DoneProcIMU(IMU_Data& Data)
 {
   DataIMU = Data;
@@ -35,6 +57,8 @@ void TimerUpdateSensor()
   //if(MainDrone.BarInit) BarObj->GetAsync(DoneProcBAR);
 }
 
+Vector3 Euler;
+
 void TimerUpdateMain()
 {
   float time = GetCurrentTime();
@@ -48,6 +72,9 @@ void TimerUpdateMain()
   
   MainIRS.UpdateGyro(gyr);
   MainIRS.UpdateAccel(acc);
+  
+  Euler = QuatToEuler(MainIRS.OriQuat);
+  
   /*if(MagReady)
   {
     MagReady=false;
@@ -91,6 +118,8 @@ void TimerUpdateMain()
   if(MainDrone.ExternMagInit) MagObj->GetAsync(DoneProcMag);*/
 }
 
+static bool control_update_flag = 0;
+
 int main()
 {
   SystemClock_Config(); // 170MHz
@@ -104,8 +133,48 @@ int main()
   
   TIM6_Enable();
   
+  attitude_t attitude;
+  rc_channels rx_chs_raw;
+  rc_channels rx_chs_normalized;
+  control_channels_t ctrl_chs;
+  
+  attitude_init(&attitude);
+  receiver_init();
+  motors_init();
+  
   while (true)
   {
+    receiver_update(&rx_chs_raw);
+	  rx_chs_normalized = normalize_channels(rx_chs_raw);
     
+    if (control_update_flag)
+		{
+      control_update_flag = 0;
+      
+      Vector3 Gyro = Vector3(DataIMU.Gyr.X, DataIMU.Gyr.Y, DataIMU.Gyr.Z);
+      
+      attitude_controller_update(
+        &ctrl_chs,
+        &rx_chs_normalized,
+        &MainIRS.OriQuat,
+        &Gyro
+      );
+		}
+		
+		if (rx_chs_normalized.rc_armed)
+	  {
+      CalibDataIMU.AllowedCalib = false;
+      
+      motors_set_throttle_mix(
+        rx_chs_normalized.rc_throttle,
+        &ctrl_chs,
+        rx_chs_normalized.rc_armed
+      );
+	  }
+	  else
+		{
+			motors_turn_off();
+			CalibDataIMU.AllowedCalib = true;
+		}
   }
 }