首先，參考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;}}
}