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

Source/BSP/Inc/imu_processing.h

@@ -7,6 +7,7 @@
 #define ACCEL_SENS_SCALE_FACTOR		8192.0f
 #define GYRO_SENS_SCALE_FACTOR		16.4f
 #define PI							3.14159265359f
+#define DEG2RAD						PI / 180.0f
 
 typedef struct
 {

Source/BSP/Src/imu_processing.c

@@ -24,9 +24,9 @@ void imu_processing_init()
 	biquad_init_lpf(&accel_y_lpf, 25.0f, 500.0f);
 	biquad_init_lpf(&accel_z_lpf, 25.0f, 500.0f);
 	
-	biquad_init_lpf(&gyro_x_lpf, 25.0f, 500.0f);
+	biquad_init_lpf(&gyro_x_lpf, 100.0f, 500.0f);
-	biquad_init_lpf(&gyro_y_lpf, 25.0f, 500.0f);
+	biquad_init_lpf(&gyro_y_lpf, 100.0f, 500.0f);
-	biquad_init_lpf(&gyro_z_lpf, 25.0f, 500.0f);
+	biquad_init_lpf(&gyro_z_lpf, 100.0f, 500.0f);
 }
 
 void imu_read_scaled(imu_scaled_t* out)

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);
+                         const imu_scaled_t* imu);*/
-void TIM6_DAC_IRQHandler();
+
 
 float accel_roll_deg(const imu_scaled_t* imu);
 float accel_pitch_deg(const imu_scaled_t* imu);

Source/Control/Src/attitude.c

@@ -2,17 +2,23 @@
 #include "pid.h"
 #include <math.h>
 
+#define MAHONY_KP	2.5f
+#define MAHONY_KI	0.01f
+
+static Vector3 gyro_bias = {};
+
 volatile uint8_t imu_update_flag = 0;
 volatile uint8_t pid_update_flag = 0;
 
 float angle_kp_roll 	= 2.5f;
 float angle_kp_pitch 	= 2.5f;
+float angle_kp_yaw 		= 2.0f;
 
 pid_t pid_roll 	= {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
 pid_t pid_pitch = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
 pid_t pid_yaw 	= {.kp = 1.8f, .ki = 0.6f, .kd = 0.0f};
 
-int16_t desired_roll = 0;
+/*int16_t desired_roll = 0;
 int16_t desired_pitch = 0;
 
 float desired_roll_rate = 0.0f;
@@ -24,9 +30,159 @@ float yaw_rate_error = 0.0f;
 
 float error_roll = 0.0f;
 float error_pitch = 0.0f;
-float error_yaw = 0.0f;
+float error_yaw = 0.0f;*/
 
-void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu)
+void attitude_init(attitude_t* att)
+{
+	att->orientation.w = 1.0f;
+	att->orientation.x = 0.0f;
+	att->orientation.y = 0.0f;
+	att->orientation.z = 0.0f;
+	
+	att->gyro.x = 0.0f;
+	att->gyro.y = 0.0f;
+	att->gyro.z = 0.0f;
+}
+
+void attitude_estimator_update(attitude_t* att, const imu_scaled_t* imu)
+{
+	Vector3 gyro 	= {imu->gx, imu->gy, imu->gz};
+	Vector3 accel 	= {imu->ax, imu->ay, imu->az};
+	
+	float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
+	
+	if (norm > 0.0001f)
+	{
+		accel.x /= norm;
+		accel.y /= norm;
+		accel.z /= norm;
+		
+		Vector3 g = gravity_from_quat(&att->orientation);
+		
+		Vector3 error = 
+		{
+			accel.y*g.z - accel.z*g.y,
+			accel.z*g.x - accel.x*g.z,
+			accel.x*g.y - accel.y*g.x
+		};
+		
+		gyro_bias.x += MAHONY_KI * error.x * IMU_DT;
+		gyro_bias.y += MAHONY_KI * error.y * IMU_DT;
+		gyro_bias.z += MAHONY_KI * error.z * IMU_DT;
+		
+		gyro_bias.x = constrain(gyro_bias.x, -GYRO_BIAS_LIM, GYRO_BIAS_LIM);
+		gyro_bias.y = constrain(gyro_bias.y, -GYRO_BIAS_LIM, GYRO_BIAS_LIM);
+		gyro_bias.z = constrain(gyro_bias.z, -GYRO_BIAS_LIM, GYRO_BIAS_LIM);
+		
+		gyro.x += MAHONY_KP * error.x + gyro_bias.x;
+		gyro.y += MAHONY_KP * error.y + gyro_bias.y;
+		gyro.z += MAHONY_KP * error.z + gyro_bias.z;
+	}
+	
+	att->gyro = gyro;
+	
+	Quaternion h = {gyro.x * DEG2RAD, gyro.y * DEG2RAD, gyro.z * DEG2RAD, 0};
+	
+	Quaternion dq = QuatProd(&att->orientation, &h);
+	dq = QuatConstProd(&dq, 0.5f * IMU_DT);
+	att->orientation = QuatSum(&att->orientation, &dq);
+	
+	att->orientation = QuatNormalize(&att->orientation, 1.0f);
+}
+
+void attitude_controller_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att)
+{
+	Quaternion q_target = rx_to_quaternion(rx);
+	Quaternion q_error 	= QuatGetError(&att->orientation, &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_roll,
+		angle_error.y * angle_kp_pitch,
+		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);
+	
+	float roll_error 	= desired_rate.x - att->gyro.x;
+	float pitch_error 	= desired_rate.y - att->gyro.y;
+	float yaw_error 	= desired_rate.z - att->gyro.z;
+	
+	control->roll 	= pid_update(&pid_roll, roll_error, att->gyro.x, IMU_DT);
+	control->pitch 	= pid_update(&pid_pitch, pitch_error, att->gyro.y, IMU_DT);
+	control->yaw 	= pid_update(&pid_yaw, yaw_error, att->gyro.z, IMU_DT);
+}
+
+Quaternion rx_to_quaternion(const rc_channels* rx)
+{
+	float roll 	= int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
+	float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD;
+	float yaw 	= 0;
+	
+	Vector3 rpy = {roll, pitch, yaw};
+	
+	return QuatCreateFromEuler(&rpy);
+}
+
+Vector3 gravity_from_quat(const Quaternion* q)
+{
+	Vector3 g;
+	
+	g.x = 2 * (q->x*q->z - q->w*q->y);
+	g.y = 2 * (q->w*q->x + q->y*q->z);
+	g.z = q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z;
+	
+	return g;
+}
+
+void TIM6_DAC_IRQHandler()
+{
+	if (TIM6->SR & TIM_SR_UIF)
+	{
+		TIM6->SR &= ~TIM_SR_UIF;
+		imu_update_flag = 1;
+		pid_update_flag = 1;
+	}
+}
+
+void attitude_update(attitude_t* attitude, imu_scaled_t* imu)
+{
+	if (!imu_update_flag)
+		return;
+	
+	imu_update_flag = 0;
+	
+	imu_read_scaled(imu);
+	attitude_estimator_update(attitude, imu);
+	
+	// complementary_filter_update(attitude, imu);
+	// yaw_rate_update(attitude, imu);
+}
+
+void attitude_pid_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att)
+{
+	if (!pid_update_flag)
+		return;
+	pid_update_flag = 0;
+	attitude_controller_update(control, rx, att);
+}
+
+float constrain(float x, float min, float max)
+{
+	if (x < min) x = min; else if (x > max) x = max;
+	return x;
+}
+
+/*void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu)
 {
 	static float roll_acc;
 	static float pitch_acc;
@@ -39,21 +195,9 @@ void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu)
 	
 	att->roll = CF_ALPHA * att->roll + (1 - CF_ALPHA) * roll_acc;
 	att->pitch = CF_ALPHA * att->pitch + (1 - CF_ALPHA) * pitch_acc;
-}
+}*/
 
-void attitude_update(attitude_t* attitude, imu_scaled_t* imu)
+/*void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu)
-{
-	if (imu_update_flag)
-	{
-		imu_update_flag = 0;
-		
-		imu_read_scaled(imu);
-		complementary_filter_update(attitude, imu);
-		yaw_rate_update(attitude, imu);
-	}
-}
-
-void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu)
 {
 	attitude->yaw_rate = imu->gz;
 }
@@ -87,17 +231,7 @@ void attitude_pid_update(control_channels_t* control,
 		control->pitch = pid_update(&pid_pitch, pitch_rate_error, imu->gx, IMU_DT);
 		control->yaw   = pid_update(&pid_yaw, yaw_rate_error, imu->gz, IMU_DT);
 	}
-}
+}*/
-
-void TIM6_DAC_IRQHandler()
-{
-	if (TIM6->SR & TIM_SR_UIF)
-	{
-		TIM6->SR &= ~TIM_SR_UIF;
-		imu_update_flag = 1;
-		pid_update_flag = 1;
-	}
-}
 
 float accel_roll_deg(const imu_scaled_t* imu) {
 	// right-left

Source/INS/geometry/quaternion.c

@@ -3,7 +3,7 @@
 
 Quaternion QuatNormalize(const Quaternion* q, const float gain)
 {
-	Quaternion res;
+  Quaternion res = {};
 	
 	float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
 	
@@ -74,11 +74,12 @@ Quaternion QuatConstProd(const Quaternion* q, const float value)
 
 Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2)
 {
-	Quaternion res = {
+	Quaternion res = 
-	.x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y,
+	{
-	.y = q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x,
+		q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y,
-	.z = q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
+		q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x,
-	.w = q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
+		q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
+		q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
 	};
 	
 	
@@ -99,13 +100,13 @@ Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW)
 	return res;
 }
 
-Quaternion QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad)
+Quaternion QuatCreateFromEuler(const Vector3* eulerAngels)
 {
 	Quaternion res;
 	
-	float h_r = 0.5f * RollPitchYawRad->y;
+	float h_r = 0.5f * eulerAngels->y;
-	float h_p = 0.5f * RollPitchYawRad->x;
+	float h_p = 0.5f * eulerAngels->x;
-	float h_y = 0.5f * RollPitchYawRad->z;
+	float h_y = 0.5f * eulerAngels->z;
 	
 	float c_r = cosf(h_r), s_r = sinf(h_r);
 	float c_p = cosf(h_p), s_p = sinf(h_p);

Source/INS/geometry/quaternion.h

@@ -22,7 +22,7 @@ Quaternion QuatConstProd(const Quaternion* q, const float value);
 Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2);
 
 Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW);
-Quaternion QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad);
+Quaternion QuatCreateFromEuler(const Vector3* eulerAngels);
 Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay);
 
 

Source/main.c

@@ -34,20 +34,19 @@ int main(void)
 	
 	imu_calibrate();
 	
+	attitude_init(&attitude);
+	
 	receiver_init();
 	
 	motors_init();
 	
 	while (1)
 	{
-		attitude_update(&attitude, &imu);
-		
 		receiver_update(&rx_chs_raw);
 		rx_chs_normalized = normalize_channels(rx_chs_raw);
 		
-		attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude, &imu);
+		attitude_update(&attitude, &imu);
-		
+		attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude);
-		lidar_update(&lidar);
 		
 		if (rx_chs_normalized.rc_armed)
 		{

drone.ewp

@@ -363,6 +363,7 @@
                     <state>$PROJ_DIR$\Source\BSP\Inc</state>
                     <state>$PROJ_DIR$\Source\Control\Inc</state>
                     <state>$PROJ_DIR$\Source\INS\geometry</state>
+                    <state>$PROJ_DIR$\Source\INS</state>
                 </option>
                 <option>
                     <name>CCStdIncCheck</name>