修改PID控制算法

- 添加角度环Yaw控制
UESTC404 · Jan 2, 2018 · dc10f5e · dc10f5e
1 parent c033d9e
commit dc10f5e
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 9 deletions.
diff --git a/project/p10-ANO/application/main.c b/project/p10-ANO/application/main.c
@@ -63,16 +63,18 @@ int main(void)
 	PID_init();
 
 
-		hmc_correct();	//数据校正
+    hmc_correct();	//数据校正
+    updateHMC5883();
+    yaw_angle_PID.Desired = atan2(magY,magX) * 180/3.14;
     for (i=0;;i++) {
-				updateHMC5883();
-			
+        updateHMC5883();
+
         Attitude();
         CtrlAttiAng();
         CtrlAttiRate();
         CtrlMotorSpeed();
-			
-				yaw=atan2(magY,magX) * 180/3.14;
+
+        yaw=atan2(magY,magX) * 180/3.14;
         ANO_DT_Data_Exchange();
         my2_ANO_DT_Data_Receive_Anl();
     }

diff --git a/project/p10-ANO/application/pid.c b/project/p10-ANO/application/pid.c
@@ -7,6 +7,7 @@
 
 
 #define max_angle_pr 10
+#define max_rate_pr  10
 #define I_limit_init 20
 #define DMP_GYRO_SCALE 16.4f    // 2000deg/s , 31276/2000=16.4f
 
@@ -111,11 +112,11 @@ void CtrlAttiAng(void)
 {
     static uint32_t told = 0;
 
+    uint32_t tnow = msTimerCounter;
     roll_angle_PID.Desired  = range_trans(u16Rcvr_ch1, max_angle_pr);       // f(u16Rcvr_ch1) +
     pitch_angle_PID.Desired = range_trans(u16Rcvr_ch2, max_angle_pr);       // f(u16Rcvr_ch2) +
-    yaw_angle_PID.Desired   = 0;                                            // 假通道，没有需求
+    yaw_angle_PID.Desired  += range_trans(3000-u16Rcvr_ch4, max_rate_pr)*(tnow-told)/1000.f;
 
-    uint32_t tnow = msTimerCounter;
     PID_postion_cal(&roll_angle_PID,  roll,  tnow-told);
     PID_postion_cal(&pitch_angle_PID, pitch, tnow-told);
     PID_postion_cal(&yaw_angle_PID,   yaw,   tnow-told);
@@ -129,11 +130,11 @@ void CtrlAttiRate(void)
 
     roll_rate_PID.Desired  = roll_angle_PID.Output ;
     pitch_rate_PID.Desired = pitch_angle_PID.Output;
-    //yaw_rate_PID.Desired   = yaw_angle_PID.Output  ;
+    yaw_rate_PID.Desired   = yaw_angle_PID.Output  ;
 
     //roll_rate_PID.Desired  = range_trans(u16Rcvr_ch1, 100);
     //pitch_rate_PID.Desired = range_trans(u16Rcvr_ch2, 100);
-    yaw_rate_PID.Desired   = range_trans(3000-u16Rcvr_ch4, 60);
+    //yaw_rate_PID.Desired   = range_trans(3000-u16Rcvr_ch4, 60);
 
     uint32_t tnow = msTimerCounter;
     PID_postion_cal(&roll_rate_PID,  gyro[0] / DMP_GYRO_SCALE, tnow-told); // DMP_GYRO_SCALE