Diff: HbManager.cpp

Revision:

48:71aec693a7dc

Parent:

47:d3fa874f336e

Child:

49:048966a8264a

--- a/HbManager.cpp	Wed Feb 20 12:54:25 2019 +0000
+++ b/HbManager.cpp	Sun Feb 24 10:33:34 2019 +0000
@@ -65,19 +65,22 @@
 
     float pidRtn;
     
-    //pidRtn = att->pid2(0,nowAngle,nowRate);
-    pidRtn = att->pid2(nowTrgtAng,nowAngle,nowRate);
+    if(g_PidPara.mode == PID_0_OFF || g_PidPara.mode == PID_0_ON){
+        pidRtn = att->pid(nowTrgtAng,nowAngle,nowRate);
+    } else {
+        pidRtn = att->pid2(nowTrgtAng,nowAngle,nowRate);
+    }
 
     if(att != NULL){
-        if(gf_Print.d1.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
+        if(gf_Print.d1.bf.pp){sp.printf("PID:PP=%f,",att->getPp());}
         if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
         if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
         if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
         if(gf_Print.d1.bf.v) {sp.printf("PID:V=%f,",att->getV()); }
         if(gf_Print.d1.bf.fb){
-            sp.printf("PID FB,%f,",pidRtn);
-            sp.printf("%f,",nowAngle);
-            sp.printf("%f,",nowRate);
+            sp.printf("PID FB:%f, %f, %f, ",pidRtn, nowAngle, nowRate);
+            //sp.printf("%f,",nowAngle);
+            //sp.printf("%f,",nowRate);
         }
     }
     
@@ -93,18 +96,38 @@
     // float bl = pidRtn * 2.25;
     // float br = pidRtn * 2.25 * -1;
 
-    // 推力は回転数の二乗に比例するので、得られた望む角速度の平方根に係数かけて回転数とする
-    float tmpRtn = abs(pidRtn);
-    tmpRtn = sqrtf(tmpRtn);
-    if(pidRtn < 0){
-        tmpRtn *= -1;
+    // // 推力は回転数の二乗に比例するので、得られた望む角速度の平方根に係数かけて回転数とする
+    // float tmpRtn = abs(pidRtn);
+    // tmpRtn = sqrtf(tmpRtn);
+    // if(pidRtn < 0){
+    //     tmpRtn *= -1;
+    // }
+    // pidRtn = tmpRtn;
+    float fl, fr, bl, br;
+    //float fl = pidRtn * 2.25 * -1;
+    //float fr = pidRtn * 2.25;
+    //float bl = pidRtn * 2.25;
+    //float br = pidRtn * 2.25 * -1;
+    // float fl = pidRtn;
+    // float fr = -pidRtn;
+    // float bl = -pidRtn;
+    // float br = pidRtn;
+    if(g_PidPara.mode == PID_0_ON){
+        fl = pidRtn;
+        fr = -pidRtn;
+        bl = -pidRtn;
+        br = pidRtn;
+    } else if(g_PidPara.mode == PID_1_ON){
+        fl = pidRtn * 2.25 * -1;
+        fr = pidRtn * 2.25;
+        bl = pidRtn * 2.25;
+        br = pidRtn * 2.25 * -1;
+    }else{
+        fl = 0;
+        fr = 0;
+        bl = 0;
+        br = 0;
     }
-    pidRtn = tmpRtn;
-    
-    float fl = pidRtn * 2.25 * -1;
-    float fr = pidRtn * 2.25;
-    float bl = pidRtn * 2.25;
-    float br = pidRtn * 2.25 * -1;
     gf_MtReq[0].val = (INT16)fl;
     gf_MtReq[1].val = (INT16)fr;
     gf_MtReq[2].val = (INT16)bl;
@@ -112,6 +135,9 @@
     for(int i = 0; i < 4; ++i){
         gf_MtReq[i].req = true;
     }
+    // if(gf_Print.d1.bf.fb){
+    //     sp.printf("REQ RPM FL:%d, FR:%d, RL:%d, RR:%d",gf_MtReq[0].val,gf_MtReq[1].val,gf_MtReq[2].val,gf_MtReq[3].val);
+    // }
 }
 
 void HbManager::controlAttitude(float cmdAngle){
@@ -242,6 +268,10 @@
         }
     }
 
+    for(int i = 0; i < 4; ++i){
+        subProp[i]->setValue();
+    }
+
     if(gf_Print.d1.bf.m1){ sp.printf("m1=%d,",(INT16)subProp[0]->getValue(ATT)); }
     if(gf_Print.d1.bf.m2){ sp.printf("m2=%d,",(INT16)subProp[1]->getValue(ATT)); }
     if(gf_Print.d1.bf.m3){ sp.printf("m3=%d,",(INT16)subProp[2]->getValue(ATT)); }