使用mpu6500, PID，互补滤波实现一个简单的飞行自稳控制系统

首先，参考ai给出的客机飞机的比较平稳的最大仰府，偏转，和防滚角度，如下：

客机的最大平稳仰俯（Pitch）、偏转（Yaw）和防滚（Roll）角度，通常在飞机的设计和飞行规程中有所规定。这些角度的限制是为了确保飞机的安全性，并在复杂的气流条件下进行平稳飞行。

具体而言：

• 仰俯（Pitch）：

  指飞机机头向上或向下的角度变化。客机设计通常会限制仰俯角度，以避免飞机失控或结构损坏。一般而言，客机的最大仰俯角在10度到20度之间，具体取决于飞机类型和飞行阶段。

• 偏转（Yaw）：

  指飞机机头左右偏离飞行方向的角度变化。偏转角度的限制是为了防止飞机剧烈摇摆或失控。客机的最大偏转角度通常在10度到20度之间，具体取决于飞机类型和飞行阶段。

• 防滚（Roll）：

  指飞机机翼左右倾斜的角度变化。防滚角度的限制是为了防止飞机翻滚或失控。客机的最大防滚角度通常在30度到40度之间，具体取决于飞机类型和飞行阶段。

需要注意的是，这些角度是飞机设计和飞行规程的限制，而不是飞机的实际飞行。在飞机的正常飞行过程中，这些角度通常不会超过设计限制。

客机的最大平稳仰俯、偏转和防滚角度通常在10度到40度之间，具体取决于飞机类型和飞行阶段。这些限制是为了确保飞机的安全性，并在复杂的气流条件下进行平稳飞行。

还让ai以大型运输机查出了最大的安全角速度，如下：

根据大型运输机设计标准，对于客机平稳安全的最大角速度变化率， 设定了以下安全限制：

- 俯仰最大安全角速度：10度/秒
- 横滚最大安全角速度：25度/秒（参考大型运输机20-30度/秒的标准）
- 偏航最大安全角速度：15度/秒

我们取:  最大仰俯角是+-20角，翻滚+-40度，偏航是+-20度

为了简单，

1. 只是加速度acc和角度gyro这个六个mpu返回的值进行计算。 不使用dmp解算姿态。

2. 使用积分的办法去粗略计算yaw, pitch和roll 使用gyro和acc直接计算。

3. 把用户的三个输入(pwm 1000-2000), 转化成目标角度，即飞机的最大仰俯角是+-20角，翻滚+-40度，偏航是+-20度

4. 使用互补滤波和PID ，计算出输出的pwm去控制舵面，最终令到俯角，翻滚和偏航达到用户输入出的值。

5. 不考虑油门对速度的影响

6. 进行单元测试，输出的pwm变成舵面角度基于物理学转化成对于加速度acc和角度gyro的影响，内容在update_sensor_by_control

7. 估算当前舵面角度和速度能产生的最大角速度
8. 比较这个最大角速度与安全角速度限制
9. 如果舵面效果不足（例如在低速时），自动增加油门提高速度，从而增强舵面效果
10. 如果舵面效果过强（例如在高速时），自动减小油门降低速度，避免过大的角速度变化
这种自适应控制方式能够在保证飞行安全和舒适性的同时，尽可能快速地达到目标姿态。

代码如下：

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>/*** @brief 互补滤波器结构体定义*/typedef struct {float alpha;     /**< 互补滤波系数，范围0-1，越接近1越信任陀螺仪数据 */float angle;     /**< 当前角度 */
} complementary_filter_t;/*** @brief 初始化互补滤波器* @param[in] *filter 指向互补滤波器结构体的指针* @param[in] alpha 互补滤波系数，范围0-1，越接近1越信任陀螺仪数据* @note 无*/void complementary_filter_init(complementary_filter_t *filter, float alpha) {filter->alpha = alpha;filter->angle = 0.0f;}/*** @brief 互补滤波计算* @param[in] *filter 指向互补滤波器结构体的指针* @param[in] acc_angle 从加速度计计算的角度* @param[in] gyro_rate 陀螺仪测量的角速度* @param[in] dt 时间间隔（秒）* @return 滤波后的角度值* @note 无*/float complementary_filter_update(complementary_filter_t *filter, float acc_angle, float gyro_rate, float dt) {// 互补滤波核心算法// 陀螺仪积分得到角度变化float gyro_angle = filter->angle + gyro_rate * dt;// 互补滤波：结合陀螺仪和加速度计的数据// alpha决定了对陀螺仪数据的信任程度filter->angle = filter->alpha * gyro_angle + (1.0f - filter->alpha) * acc_angle;return filter->angle;}// 全局互补滤波器实例
static complementary_filter_t gs_pitch_filter;
static complementary_filter_t gs_roll_filter;
static complementary_filter_t gs_yaw_filter;
static float gs_prev_yaw = 0.0f;
static float gs_prev_time = 0.0f;// PID控制器参数和状态变量
typedef struct {float kp;           // 比例系数float ki;           // 积分系数float kd;           // 微分系数float error_sum;    // 误差累积float prev_error;   // 上一次误差float output_limit; // 输出限制
} pid_controller_t;// 全局PID控制器实例
static pid_controller_t gs_pitch_pid;
static pid_controller_t gs_roll_pid;
static pid_controller_t gs_yaw_pid;/*** @brief 初始化互补滤波器* @note 无*/
void init_filters(void)
{// 初始化俯仰角和横滚角的互补滤波器// alpha=0.98表示98%信任陀螺仪数据，2%信任加速度计数据complementary_filter_init(&gs_pitch_filter, 0.98f);complementary_filter_init(&gs_roll_filter, 0.98f);// 初始化偏航角的互补滤波器// 对于偏航角，我们更依赖陀螺仪数据，因此alpha设置得更高complementary_filter_init(&gs_yaw_filter, 0.99f);// 初始化时间和偏航角记录gs_prev_time = 0.0f;gs_prev_yaw = 0.0f;
}/*** @brief 初始化PID控制器* @param[out] pid 指向PID控制器结构体的指针* @param[in] kp 比例系数* @param[in] ki 积分系数* @param[in] kd 微分系数* @param[in] output_limit 输出限制值* @note 无*/
void pid_init(pid_controller_t *pid, float kp, float ki, float kd, float output_limit)
{pid->kp = kp;pid->ki = ki;pid->kd = kd;pid->error_sum = 0.0f;pid->prev_error = 0.0f;pid->output_limit = output_limit;
}/*** @brief 计算PID控制器输出* @param[in,out] pid 指向PID控制器结构体的指针* @param[in] error 当前误差* @param[in] dt 时间间隔（秒）* @return PID控制器输出值* @note 无*/
float pid_compute(pid_controller_t *pid, float error, float dt)
{// 计算积分项pid->error_sum += error * dt;// 计算微分项float error_diff = (error - pid->prev_error) / dt;// 计算PID输出float output = pid->kp * error + pid->ki * pid->error_sum + pid->kd * error_diff;// 限制输出范围if (output > pid->output_limit) {output = pid->output_limit;} else if (output < -pid->output_limit) {output = -pid->output_limit;}// 保存当前误差用于下次计算pid->prev_error = error;return output;
}/*** @brief 初始化直线水平前进PID控制器* @note 无*/
void init_simple_stable_pid(void)
{// 初始化俯仰角PID控制器pid_init(&gs_pitch_pid, 0.5f, 0.1f, 0.05f, 10.0f);// 初始化横滚角PID控制器pid_init(&gs_roll_pid, 0.5f, 0.1f, 0.05f, 10.0f);// 初始化偏航角PID控制器pid_init(&gs_yaw_pid, 0.8f, 0.2f, 0.1f, 10.0f);// 初始化互补滤波器init_filters();
}/*** @brief 从加速度数据计算俯仰角和横滚角* @param[in] acc 加速度数据数组[3]* @param[out] pitch 计算得到的俯仰角（度）* @param[out] roll 计算得到的横滚角（度）* @note 无*/void calculate_angles_from_acc(float acc[3], float *pitch, float *roll){// 计算俯仰角（pitch）和横滚角（roll）// 注意：这里假设z轴垂直于地面，x轴向前，y轴向右// 计算俯仰角（绕y轴旋转）*pitch = atan2f(-acc[0], sqrtf(acc[1] * acc[1] + acc[2] * acc[2])) * 180.0f / M_PI;// 计算横滚角（绕x轴旋转）*roll = atan2f(acc[1], acc[2]) * 180.0f / M_PI;}/*** @brief 使用PID控制算法计算直线水平前进的控制输出* @param[in] acc 加速度数据数组[3]* @param[in] gyro 陀螺仪数据数组[3]* @param[in] target_pitch 目标俯仰角* @param[in] target_roll 目标横滚角* @param[in] target_yaw_rate 目标偏航角变化率* @param[out] control_output 控制输出数组[3]，分别对应俯仰、横滚和偏航的控制量* @param[in,out] throttle_pwm 油门PWM值，范围1000-2000，函数会根据需要调整此值* @return 是否已经达到直线水平前进状态* @note 此函数需要在初始化PID控制器后使用*/
void simple_stable_pid_control(float acc[3], float gyro[3], float target_pitch, float target_roll, float target_yaw_rate, float control_output[3], int *throttle_pwm)
{float pitch_acc, roll_acc;float current_time;float dt;float pitch, roll;float yaw, yaw_gyro; // 使用陀螺仪积分计算偏航角// 获取当前时间（秒），实际应用中需要替换为真实的时间获取函数current_time = 0.01f + gs_prev_time; // 假设每次调用间隔10msdt = current_time - gs_prev_time;// 使用陀螺仪z轴数据积分计算当前偏航角// gyro[2]是z轴角速度，单位为度/秒，积分得到角度变化yaw_gyro = gs_prev_yaw + gyro[2] * dt;// 从加速度计算出角度calculate_angles_from_acc(acc, &pitch_acc, &roll_acc);// 使用互补滤波更新角度pitch = complementary_filter_update(&gs_pitch_filter, pitch_acc, gyro[0], dt);roll = complementary_filter_update(&gs_roll_filter, roll_acc, gyro[1], dt);// 对偏航角也应用互补滤波// 注意：由于没有从加速度计直接获取偏航角的方法，这里使用陀螺仪积分值作为主要输入// 在实际应用中，可以考虑使用磁力计数据作为偏航角的参考yaw = complementary_filter_update(&gs_yaw_filter, yaw_gyro, gyro[2], dt);// 计算偏差 - 使用传入的目标值而不是固定的0float pitch_error = target_pitch - pitch; // 目标俯仰角float roll_error = target_roll - roll;    // 目标横滚角float yaw_rate = (yaw - gs_prev_yaw) / dt; // 当前偏航角变化率float yaw_error = target_yaw_rate - yaw_rate; // 目标偏航角变化率// 使用PID控制器计算控制输出control_output[0] = pid_compute(&gs_pitch_pid, pitch_error, dt); // 俯仰控制control_output[1] = pid_compute(&gs_roll_pid, roll_error, dt);   // 横滚控制control_output[2] = pid_compute(&gs_yaw_pid, yaw_error, dt);     // 偏航控制// 定义客机平稳安全的最大角速度变化率（度/秒）// 根据大型运输机设计标准，最大滚转速率通常在20-30度/秒之间const float MAX_SAFE_PITCH_RATE = 10.0f;  // 俯仰最大安全角速度（度/秒）const float MAX_SAFE_ROLL_RATE = 25.0f;   // 横滚最大安全角速度（度/秒）const float MAX_SAFE_YAW_RATE = 15.0f;    // 偏航最大安全角速度（度/秒）// 计算当前角速度（使用陀螺仪数据）float current_pitch_rate = fabsf(gyro[0]); // X轴陀螺仪数据对应俯仰角速度float current_roll_rate = fabsf(gyro[1]);  // Y轴陀螺仪数据对应横滚角速度float current_yaw_rate = fabsf(gyro[2]);   // Z轴陀螺仪数据对应偏航角速度// 计算控制输出对应的舵面角度float elevator_angle = control_output[0] * 4.5f; // 假设控制输出1对应4.5度舵面角度float aileron_angle = control_output[1] * 4.5f;float rudder_angle = control_output[2] * 4.5f;// 估计舵面能产生的最大角速度（简化模型）// 当前油门值（1000-2000）转换为百分比（0-100%）float throttle_percent = (*throttle_pwm - 1000) / 10.0f;// 假设速度与油门成正比，舵面效果与速度平方成正比（伯努利原理）float speed_factor = 1.0f + (throttle_percent * throttle_percent) * 0.0001f;// 估计当前舵面能产生的最大角速度float max_pitch_rate_possible = fabsf(elevator_angle) * 0.5f * speed_factor;float max_roll_rate_possible = fabsf(aileron_angle) * 0.4f * speed_factor;float max_yaw_rate_possible = fabsf(rudder_angle) * 0.3f * speed_factor;// 检查是否需要调整油门bool need_more_throttle = false;bool need_less_throttle = false;// 如果需要的角速度超过舵面能产生的最大角速度，需要增加油门if (max_pitch_rate_possible < MAX_SAFE_PITCH_RATE && fabsf(pitch_error) > 5.0f) {need_more_throttle = true;}if (max_roll_rate_possible < MAX_SAFE_ROLL_RATE && fabsf(roll_error) > 5.0f) {need_more_throttle = true;}if (max_yaw_rate_possible < MAX_SAFE_YAW_RATE && fabsf(yaw_error) > 5.0f) {need_more_throttle = true;}// 如果当前角速度已经超过安全角速度，但舵面角度很小，需要减小油门if (current_pitch_rate > MAX_SAFE_PITCH_RATE && fabsf(elevator_angle) < 10.0f) {need_less_throttle = true;}if (current_roll_rate > MAX_SAFE_ROLL_RATE && fabsf(aileron_angle) < 10.0f) {need_less_throttle = true;}if (current_yaw_rate > MAX_SAFE_YAW_RATE && fabsf(rudder_angle) < 10.0f) {need_less_throttle = true;}// 调整油门值if (need_more_throttle && !need_less_throttle) {// 增加油门，但不超过最大值2000*throttle_pwm += 20;if (*throttle_pwm > 2000) *throttle_pwm = 2000;} else if (need_less_throttle && !need_more_throttle) {// 减小油门，但不低于最小值1000*throttle_pwm -= 20;if (*throttle_pwm < 1000) *throttle_pwm = 1000;}// 更新时间和偏航角记录 gs_prev_time = current_time;gs_prev_yaw = yaw;
}/*** @brief PWM值转换为角度* @param[in] pwm PWM值，范围1000-2000* @param[in] channel 通道号，0=俯仰(±20°)，1=横滚(±40°)，2=偏航(±20°)* @return 转换后的角度值* @note 根据不同通道返回不同范围的角度值*/
float pwm_to_angle(int pwm, int channel)
{// 中点值1500对应0度float normalized = (pwm - 1500) / 500.0f; // 归一化到±1范围// 根据通道返回不同范围的角度switch(channel) {case 0: // 俯仰通道，±20度return normalized * 20.0f;case 1: // 横滚通道，±40度return normalized * 40.0f;case 2: // 偏航通道，±20度return normalized * 20.0f;default:return normalized * 20.0f; // 默认±20度}
}/*** @brief 控制输出转换为PWM值* @param[in] control 控制输出值* @return 转换后的PWM值，范围1000-2000* @note 无*/
int control_to_pwm(float control)
{// 将控制输出(-10到10)线性映射到PWM值(1000-2000)// 首先将控制输出限制在-10到10的范围内if (control > 10.0f) control = 10.0f;if (control < -10.0f) control = -10.0f;// 然后映射到1000-2000return 1500 + (int)(control * 50.0f);
}/*** @brief PWM值转换为舵面角度* @param[in] pwm PWM值，范围1000-2000* @return 转换后的舵面角度值，范围-45到+45度* @note 无*/
float pwm_to_control_surface_angle(int pwm)
{// 将PWM值(1000-2000)线性映射到舵面角度值(-45到+45度)return -45.0f + (pwm - 1000) * 90.0f / 1000.0f;
}/*** @brief 根据舵面角度更新传感器数据* @param[in,out] acc 加速度数据数组[3]* @param[in,out] gyro 陀螺仪数据数组[3]* @param[in] output_pwm 输出PWM值数组[3]，分别对应俯仰、横滚和偏航通道* @param[in] dt 时间间隔（秒）* @note 根据物理公式模拟舵面角度对加速度和陀螺仪数据的影响*/
void update_sensor_by_control(float acc[3], float gyro[3], int output_pwm[3], int throttle_pwm, float dt)
{// 将PWM值转换为舵面角度（-45到+45度）float elevator_angle = pwm_to_control_surface_angle(output_pwm[0]); // 升降舵角度（俯仰）float aileron_angle = pwm_to_control_surface_angle(output_pwm[1]);  // 副翼角度（横滚）float rudder_angle = pwm_to_control_surface_angle(output_pwm[2]);   // 方向舵角度（偏航）// 计算飞行速度（基于油门值）// 油门范围1000-2000，转换为0-100%的油门量float throttle_percent = (throttle_pwm - 1000) / 10.0f;// 假设最大速度为30m/s，根据油门百分比计算当前速度float airspeed = throttle_percent * 0.3f; // 0-30 m/s// 模拟舵面角度对陀螺仪数据的影响// 舵面角度越大，产生的角速度越大// 这里使用简化的物理模型，实际上应该考虑更复杂的空气动力学模型// 计算舵面角度变化率（简化为当前角度除以时间步长）// 这个变化率用于模拟舵面移动时产生的瞬时反作用力float elevator_rate = elevator_angle / dt;float aileron_rate = aileron_angle / dt;float rudder_rate = rudder_angle / dt;// 考虑速度对舵面效果的影响（伯努利原理）// 速度越大，舵面产生的力矩越大float speed_factor = 1.0f + (airspeed * airspeed) * 0.01f; // 二次方关系，模拟动压与速度平方成正比// 升降舵影响俯仰角速度（绕Y轴）// 正角度产生向上的力矩，负角度产生向下的力矩gyro[0] += elevator_angle * 0.5f * speed_factor; // 比例因子可以根据实际情况调整// 副翼影响横滚角速度（绕X轴）// 正角度产生向右的力矩，负角度产生向左的力矩gyro[1] += aileron_angle * 0.4f * speed_factor;// 方向舵影响偏航角速度（绕Z轴）// 正角度产生向右的力矩，负角度产生向左的力矩gyro[2] += rudder_angle * 0.3f * speed_factor;// 添加舵面移动产生的反作用力对陀螺仪的影响// 舵面快速移动时会产生反向的力矩gyro[0] -= elevator_rate * 0.05f * speed_factor; // 升降舵移动产生的反作用力矩gyro[1] -= aileron_rate * 0.04f * speed_factor;  // 副翼移动产生的反作用力矩gyro[2] -= rudder_rate * 0.03f * speed_factor;   // 方向舵移动产生的反作用力矩// 模拟舵面角度对加速度数据的影响// 舵面角度会改变飞行器的姿态，从而影响加速度计测量的重力分量// 升降舵影响前后加速度（X轴）// 正角度（向上）会产生向后的加速度，负角度（向下）会产生向前的加速度acc[0] += elevator_angle * 0.01f * speed_factor;// 副翼影响左右加速度（Y轴）// 正角度（向右）会产生向右的加速度，负角度（向左）会产生向左的加速度acc[1] += aileron_angle * 0.008f * speed_factor;// 方向舵主要影响偏航，对加速度的直接影响较小，这里简化处理// 但在实际飞行中，方向舵会通过改变飞行器的侧滑角间接影响加速度acc[1] += rudder_angle * 0.003f * speed_factor;// 添加舵面移动产生的反作用力对加速度计的影响// 舵面快速移动时会产生反向的加速度acc[0] -= elevator_rate * 0.002f * speed_factor; // 升降舵移动产生的反向加速度acc[1] -= aileron_rate * 0.0015f * speed_factor; // 副翼移动产生的反向加速度acc[1] -= rudder_rate * 0.0005f * speed_factor;  // 方向舵移动产生的反向加速度// 模拟伯努利原理对传感器的影响// 速度越大，气流对传感器的影响越大// 这里简化为速度对加速度和陀螺仪的直接影响// 速度产生的振动影响（随机噪声与速度成正比）float vibration_factor = airspeed * 0.02f;acc[0] += (rand() / (float)RAND_MAX - 0.5f) * vibration_factor;acc[1] += (rand() / (float)RAND_MAX - 0.5f) * vibration_factor;acc[2] += (rand() / (float)RAND_MAX - 0.5f) * vibration_factor;gyro[0] += (rand() / (float)RAND_MAX - 0.5f) * vibration_factor * 5.0f;gyro[1] += (rand() / (float)RAND_MAX - 0.5f) * vibration_factor * 5.0f;gyro[2] += (rand() / (float)RAND_MAX - 0.5f) * vibration_factor * 5.0f;// 速度产生的稳定性影响（高速时更稳定，低速时更不稳定）// 这是因为高速时舵面效果更好，控制更精确float stability_factor = 0.0f;if (airspeed > 5.0f) { // 假设5m/s以上才有明显的稳定效果stability_factor = (airspeed - 5.0f) * 0.01f;if (stability_factor > 0.2f) stability_factor = 0.2f; // 限制最大稳定效果// 高速时减小随机扰动acc[0] *= (1.0f - stability_factor);acc[1] *= (1.0f - stability_factor);gyro[0] *= (1.0f - stability_factor);gyro[1] *= (1.0f - stability_factor);gyro[2] *= (1.0f - stability_factor);}// 模拟重力对加速度的影响（根据当前姿态）// 这里使用简化模型，假设舵面角度直接影响姿态角// 在实际情况下，应该通过积分角速度来更新姿态角// 假设当前俯仰角和横滚角与舵面角度成正比（简化模型）float pitch_angle = elevator_angle * 0.2f; // 比例因子可以根据实际情况调整float roll_angle = aileron_angle * 0.2f;// 将角度转换为弧度float pitch_rad = pitch_angle * M_PI / 180.0f;float roll_rad = roll_angle * M_PI / 180.0f;// 根据姿态角计算重力在三个轴上的分量// 这是一个简化的模型，实际上应该使用完整的旋转矩阵float gx = sinf(pitch_rad);float gy = -sinf(roll_rad) * cosf(pitch_rad);float gz = cosf(roll_rad) * cosf(pitch_rad);// 将重力分量添加到加速度中（重力加速度标准值为1g）acc[0] = 0.8f * acc[0] + 0.2f * gx;acc[1] = 0.8f * acc[1] + 0.2f * gy;acc[2] = 0.8f * acc[2] + 0.2f * gz;// 添加一些随机噪声，模拟真实传感器的噪声acc[0] += (rand() / (float)RAND_MAX - 0.5f) * 0.02f;acc[1] += (rand() / (float)RAND_MAX - 0.5f) * 0.02f;acc[2] += (rand() / (float)RAND_MAX - 0.5f) * 0.02f;gyro[0] += (rand() / (float)RAND_MAX - 0.5f) * 0.5f;gyro[1] += (rand() / (float)RAND_MAX - 0.5f) * 0.5f;gyro[2] += (rand() / (float)RAND_MAX - 0.5f) * 0.5f;
}/*** @brief 模拟读取加速度和陀螺仪数据* @param[out] acc 加速度数据数组[3]* @param[out] gyro 陀螺仪数据数组[3]* @note 在实际应用中，这个函数应该从传感器读取真实数据*/
void read_sensor_data(float acc[3], float gyro[3])
{// 模拟数据 - 在实际应用中应该从传感器读取// 这里假设飞行器处于近似水平状态，有轻微扰动// 加速度数据 (x, y, z) 单位: g// 理想情况下，水平静止时 acc = {0, 0, 1}static float time_counter = 0.0f;time_counter += 0.02f; // 每次增加20ms// 添加一些随机扰动和周期性变化模拟真实环境acc[0] = 0.05f * sinf(time_counter * 0.5f); // 轻微前后晃动acc[1] = 0.03f * cosf(time_counter * 0.7f); // 轻微左右晃动acc[2] = 0.98f + 0.02f * sinf(time_counter * 0.3f); // 接近1g，有轻微上下波动// 陀螺仪数据 (x, y, z) 单位: 度/秒// 理想情况下，静止时 gyro = {0, 0, 0}gyro[0] = 2.0f * cosf(time_counter * 0.6f); // 轻微俯仰角速度gyro[1] = 1.5f * sinf(time_counter * 0.8f); // 轻微横滚角速度gyro[2] = 1.0f * sinf(time_counter * 0.4f); // 轻微偏航角速度
}/*** @brief 模拟读取PWM输入通道* @param[out] pwm_channels PWM值数组[4]，分别对应俯仰、横滚、偏航和油门通道* @note 在实际应用中，这个函数应该从接收机读取真实PWM值*/
void read_pwm_channels(int pwm_channels[4])
{// 模拟数据 - 在实际应用中应该从接收机读取// 这里假设接收到的PWM值在中间位置附近有轻微变化static float time_counter = 0.0f;time_counter += 0.02f; // 每次增加20ms// 添加一些随机扰动和周期性变化模拟真实环境// 中间值1500附近波动pwm_channels[0] = 1500 + (int)(50.0f * sinf(time_counter * 0.3f)); // 俯仰通道pwm_channels[1] = 1500 + (int)(40.0f * cosf(time_counter * 0.5f)); // 横滚通道pwm_channels[2] = 1500 + (int)(30.0f * sinf(time_counter * 0.7f)); // 偏航通道pwm_channels[3] = 1500 + (int)(100.0f * sinf(time_counter * 0.2f)); // 油门通道
}/*** @brief 判断PID控制是否达到稳定状态* @param[in] control_output 控制输出数组[3]* @return 如果所有控制输出都小于阈值，返回1，否则返回0* @note 无*/
int is_pid_stable(float control_output[3])
{// 定义稳定阈值const float threshold = 0.5f;// 检查所有控制输出是否都小于阈值if (fabsf(control_output[0]) < threshold &&fabsf(control_output[1]) < threshold &&fabsf(control_output[2]) < threshold) {return 1; // 稳定}return 0; // 不稳定
}/*** @brief 稳定飞行单元测试函数* @note 此函数包含一个无限循环，模拟飞行控制系统的运行*/
void stable_flight_unit_test(void)
{// 初始化PID控制器init_simple_stable_pid();// 定义变量float acc[3];          // 加速度数据float gyro[3];         // 陀螺仪数据int pwm_channels[4];   // PWM输入通道，包括油门float target_angles[3]; // 目标角度float control_output[3]; // 控制输出int output_pwm[3];     // 输出PWM值int stable_count = 0;   // 稳定计数器int iteration = 0;      // 迭代计数器float dt = 0.02f;      // 时间步长，20ms// 初始化输出PWM值为中间值（1500）output_pwm[0] = 1500;output_pwm[1] = 1500;output_pwm[2] = 1500;// 初始化随机数生成器srand(time(NULL));printf("开始稳定飞行单元测试...\n");// 无限循环，每次迭代模拟0.02秒while (1) {// 1. 读取当前的加速度和陀螺仪数据read_sensor_data(acc, gyro);// 1.1 根据上一次的控制输出和当前油门值更新传感器数据，模拟舵面和速度对传感器的物理影响update_sensor_by_control(acc, gyro, output_pwm, pwm_channels[3], dt);// 2. 读取三个通道的PWM值(1000-2000)read_pwm_channels(pwm_channels);// 将PWM值线性转换为目标角度target_angles[0] = pwm_to_angle(pwm_channels[0], 0); // 俯仰目标角度 (±20°)target_angles[1] = pwm_to_angle(pwm_channels[1], 1); // 横滚目标角度 (±40°)target_angles[2] = pwm_to_angle(pwm_channels[2], 2); // 偏航目标角度变化率 (±20°)// 3. 调用PID控制函数，并传递油门值进行自动调整simple_stable_pid_control(acc, gyro, target_angles[0], target_angles[1], target_angles[2], control_output,&pwm_channels[3]);// 4. 将控制输出转换为PWM值(1000-2000)并打印结果output_pwm[0] = control_to_pwm(control_output[0]);output_pwm[1] = control_to_pwm(control_output[1]);output_pwm[2] = control_to_pwm(control_output[2]);// 打印当前迭代的信息printf("迭代 %d:\n", iteration);printf("  输入 PWM: [%d, %d, %d, %d]\n", pwm_channels[0], pwm_channels[1], pwm_channels[2], pwm_channels[3]);printf("  目标角度: [%.2f, %.2f, %.2f]\n", target_angles[0], target_angles[1], target_angles[2]);printf("  控制输出: [%.2f, %.2f, %.2f]\n", control_output[0], control_output[1], control_output[2]);// 记录调整前的油门值，用于检测是否被自动调整static int prev_throttle = 0;if (iteration == 0) {prev_throttle = pwm_channels[3];}// 显示油门信息，包括是否被自动调整printf("  油门: %d (%.1f%%)", pwm_channels[3], (pwm_channels[3] - 1000) / 10.0f);if (pwm_channels[3] > prev_throttle) {printf(" [自动增加 +%d]\n", pwm_channels[3] - prev_throttle);} else if (pwm_channels[3] < prev_throttle) {printf(" [自动减少 -%d]\n", prev_throttle - pwm_channels[3]);} else {printf(" [未调整]\n");}prev_throttle = pwm_channels[3];printf("  输出 PWM: [%d, %d, %d]\n", output_pwm[0], output_pwm[1], output_pwm[2]);// 5. 检查PID是否接近稳定if (is_pid_stable(control_output)) {stable_count++;printf("  状态: 稳定 (%d/5)\n", stable_count);// 如果连续5次稳定，则退出循环if (stable_count >= 5) {printf("\n稳定飞行测试成功! PID控制器已达到稳定状态.\n");break;}} else {stable_count = 0;printf("  状态: 不稳定\n");}printf("\n");// 增加迭代计数器iteration++;// 模拟延时0.02秒// 在实际应用中，应该使用真实的延时函数// delay_ms(20);// 为了防止无限循环，设置最大迭代次数if (iteration >= 100) {printf("\n达到最大迭代次数，测试结束。PID控制器未能达到稳定状态.\n");break;}}
}