Diff: userTask.cpp

Revision:

47:d3fa874f336e

Parent:

46:5074781a28dd

Child:

48:71aec693a7dc

--- a/userTask.cpp	Tue Feb 19 11:52:49 2019 +0000
+++ b/userTask.cpp	Wed Feb 20 12:54:25 2019 +0000
@@ -206,12 +206,12 @@
                 else {
 
                     if(hb.chkInRangeIDLE()){
-                        hb.calAtt();//現在値でヨー角校正
+                        //hb.calAtt();//現在値でヨー角校正
                         setState(IDLE);
                     }
                     else{
                         // デバッグのためここもスルー
-                        hb.calAtt();//現在値でヨー角校正
+                        //hb.calAtt();//現在値でヨー角校正
                         setState(IDLE);
                     }
                 }
@@ -232,9 +232,24 @@
                 bDoCtrlMot = true;
                 //bDoCtrlAtt = true;
             break;
+            case UPPER_IDLE:
+                if(gf_StateEnt){
+                    sp.printf("UPPER IDLE state\r\n");
+                    hb.calAtt();//現在値でヨー角校正
+                    gf_StateEnt = false;
+                }
+                //SWのチェック
+                hb.chkSW(UPPER_IDLE);
+                AxlRpm = hb.getUserMotAxl();
+                hb.setMotVal(R_L,AxlRpm);
+                hb.setMotVal(R_R,AxlRpm);
+                bDoCtrlEng = true;
+                bDoCtrlMot = true;
+            break;
             case TAKE_OFF:
                 if(gf_StateEnt){
                     sp.printf("TAKE_OFF state\r\n");
+                    hb.calAtt();//現在値でヨー角校正
                     gf_StateEnt = false;
                 }
                 // エンジン回転数のチェック
@@ -452,7 +467,6 @@
         //hb.controlMotor();//モーター指令出し
         if(bDoCtrlAtt)hb.controlAttitude(); //姿勢制御
         if(bDoCtrlMot)hb.controlMotor();//モーター指令出し
-        //if(bDoCtrlEng)hb.controlEngine();//エンジン指令出し
         if(bDoCtrlEng)hb.controlEngine(gf_State);//エンジン指令出し
 
         if(gf_Print.d1.bf.stat){