Осуществлён переход на кватернионы. Но нужно протестировать

Source/Control/Inc/attitude.h

@@ -4,24 +4,49 @@
 #include "imu_processing.h"
 #include "radio_receiver.h"
 #include "pid.h"
+#include "quaternion.h"
+#include "vector.h"
 
 #define CF_ALPHA		0.99f
+#define GYRO_BIAS_LIM	0.5f
+#define RATE_LIM		300.0f
 
-typedef struct
+/*typedef struct
 {
 	float roll;			// deg
 	float pitch;		// deg
 	float yaw_rate;		// deg/s
+} attitude_t;*/
+
+typedef struct
+{
+	Quaternion orientation;	// current orientation
+	Vector3 gyro;			// current angular velocity
 } attitude_t;
 
-void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu);
+
+void attitude_init(attitude_t* att);
+void attitude_estimator_update(attitude_t* att, const imu_scaled_t* imu);
+void attitude_controller_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att);
+
+void TIM6_DAC_IRQHandler();
 void attitude_update(attitude_t* attitude, imu_scaled_t* imu);
+void attitude_pid_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att);
+
+Vector3 gravity_from_quat(const Quaternion* q);
+Quaternion rx_to_quaternion(const rc_channels* rx);
+
+float constrain(float x, float min, float max);
+
+///////////////////////////////////////////////////////////////////////////////
+
+void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu);
 void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu);
-void attitude_pid_update(control_channels_t* control, 
+/*void attitude_pid_update(control_channels_t* control, 
                          const rc_channels* rx, 
                          const attitude_t* att, 
-                         const imu_scaled_t* imu);
-void TIM6_DAC_IRQHandler();
+                         const imu_scaled_t* imu);*/
+
 
 float accel_roll_deg(const imu_scaled_t* imu);
 float accel_pitch_deg(const imu_scaled_t* imu);