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

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

Source/Control/Attitude.cpp

@@ -0,0 +1,73 @@
+#include "Attitude.h"
+#include "PID.h"
+#include <math.h>
+
+#define FREQUENCY     100.0f
+#define PERIOD        1.0f / FREQUENCY
+
+#define PI            3.14159265359f
+#define DEG2RAD       PI / 180.0f
+#define RATE_LIM      300.0f
+
+constexpr float angle_kp_pitch = 2.5f;
+constexpr float angle_kp_roll  = 2.5f;
+constexpr float angle_kp_yaw   = 2.0f;
+
+pid_t pid_pitch = {.kp = 0.6f, .kd = 0.025f};
+pid_t pid_roll  = {.kp = 0.6f, .kd = 0.025f};
+pid_t pid_yaw   = {.kp = 0.6f};
+
+void attitude_init(attitude_t* att)
+{
+  att->gyro = (Vector3){};
+}
+
+void attitude_controller_update(control_channels_t* control,
+                                const rc_channels* rx,
+                                const Quaternion* current_q,
+                                const Vector3* gyro)
+{
+  Quaternion q_target = rx_to_quaternion(rx);
+  
+  Quaternion q_error = current_q->GetError(q_target, true);
+
+  Vector3 angle_error =
+  {
+    2.0f * q_error.X,
+    2.0f * q_error.Y,
+    2.0f * q_error.Z
+  };
+
+  Vector3 desired_rate =
+  {
+    angle_error.X * angle_kp_pitch,
+    angle_error.Y * angle_kp_roll,
+    angle_error.Z * angle_kp_yaw
+  };
+
+  desired_rate.X = constrain(desired_rate.X, -RATE_LIM, RATE_LIM);
+  desired_rate.Y = constrain(desired_rate.Y, -RATE_LIM, RATE_LIM);
+  desired_rate.Z = constrain(desired_rate.Z, -RATE_LIM, RATE_LIM);
+
+  control->pitch = pid_update(&pid_pitch, desired_rate.X - gyro->X, gyro->X, PERIOD);
+  control->roll  = pid_update(&pid_roll,  desired_rate.Y - gyro->Y, gyro->Y, PERIOD);
+  control->yaw   = pid_update(&pid_yaw,   desired_rate.Z - gyro->Z, gyro->Z, PERIOD);
+}
+
+Quaternion rx_to_quaternion(const rc_channels* rx)
+{
+  Quaternion q;
+	float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD;
+	float roll 	= int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
+	float yaw 	= 0;
+	
+	Vector3 pry = {pitch, roll, yaw};
+	q.CreatePitchRollYaw(pry);
+	return q;
+}
+
+float constrain(float x, float min, float max)
+{
+	if (x < min) x = min; else if (x > max) x = max;
+	return x;
+}