Diff: HbAttitude.cpp

Revision:

32:7f4145cc3551

Parent:

31:56c554c560c1

Child:

47:d3fa874f336e

--- a/HbAttitude.cpp	Wed Dec 19 12:22:22 2018 +0000
+++ b/HbAttitude.cpp	Thu Dec 20 11:38:09 2018 +0000
@@ -43,35 +43,35 @@
 {
     //エラー量：指令値との差を求める
     float errAng = iCmdAng - iCurAng;
-    //アウターループKtゲインを掛けて、角速度にkvゲインをかけて、目標角速度とする
+    //アウターループPpゲインを掛けて、角速度にkvゲインをかけて、目標角速度とする
     float AngVelo = (iCurAng - oAng) * 50;//サンプリング周期をかけて、角速度(deg/s)に
     float cmdRate= errAng * p - AngVelo * kv;
     
-    // //▼角速度偏差（指令値と現在値との差）
-    // float devRate = cmdRate - iRate ;
+    //▼角速度偏差（指令値と現在値との差）
+    float devRate = cmdRate - iRate ;
 
-    // //▼比例項
-    // float clcP = devRate * kp; 
+    //▼比例項
+    float clcP = devRate * kp; 
     
-    // //▼積分項
-    // float xi = ki * devRate;  //係数をかける
-    // float tmpInteg = sum + xi;//積分して
+    //▼積分項
+    float xi = ki * devRate;  //係数をかける
+    float tmpInteg = sum + xi;//積分して
 
-    // //リミット掛ける
-    // if(tmpInteg > limitH){tmpInteg = limitH;}
-    // if(tmpInteg < limitL){tmpInteg = limitL;}
+    //リミット掛ける
+    if(tmpInteg > limitH){tmpInteg = limitH;}
+    if(tmpInteg < limitL){tmpInteg = limitL;}
 
-    // //積分値を次回計算用に保存
-    // sum = tmpInteg;
+    //積分値を次回計算用に保存
+    sum = tmpInteg;
     
-    // //▼微分項
-    // float clcD = kd * (devRate - old);
-    //過去データ書き換え
-    // old = devRate;
+    //▼微分項
+    float clcD = kd * (devRate - old);
+    // 過去データ書き換え
+    old = devRate;
     oAng = iCurAng;
     //
-    // return  clcP + tmpInteg + clcD;
-    return cmdRate;
+    return  clcP + tmpInteg + clcD;
+    //return cmdRate;
 }
 
 //=========================================================