File content as of revision 26:732bc37fbefd:

//RTOS関連------------------
#include "FreeRTOS.h"
#include "task.h"
//#include "queue.h"
//------------------RTOS関連

#include "globalFlags.h"
#include "HbManager.h"
#include "hbCommand.h"
#include "uart.h"
#include "fpga.h"

//タスクハンドル（停止とか再開に必要）
static xTaskHandle  tskHandle[3]={NULL,};

//------------------
//タスク停止
//------------------
static void taskStop(int iId){
    sp.printf("Task[%d] Stop\r\n" , iId);
    vTaskSuspend(tskHandle[iId]);//タスクを止める
}

//------------------
//タスク再開
//------------------
static void taskStart(int iId){
    sp.printf("Task[%d] Start!!\r\n" , iId);
    vTaskResume(tskHandle[iId]);//タスク再開
}

//========================================================
//コマンド解析タスク
//========================================================
void taskCmdParser(void *pvParameters){
//    int8_t          *pcTaskName = (int8_t *) pvParameters;;
    portTickType    xLastWakeTime = xTaskGetTickCount();
    
//    vTaskDelay(300);//制御タスクを先に走らせたいので待たす
    
    while(1){
        led2=!led2;
        //コマンド解釈
        commandParse();
        
        //
//        if(gf_Armed){
//            setState(WAKEUP);
//            gf_Armed = false;
//            // taskStop(0);//制御タスクを止める
//            // taskStart(2);//デバッグタスク再開
//        }
        // else{
        //     taskStop(2);//デバッグタスクを止める
        //     taskStart(0);//制御タスク再開
        // }//switch

        //次の周期まで待つ
        vTaskDelayUntil(&xLastWakeTime, 50 / portTICK_RATE_MS );
    }
}
//========================================================
//ホバーバイク制御タスク
//========================================================
void taskHbControl(void *pvParameters){
//    int8_t          *pcTaskName = (int8_t *) pvParameters;;
    portTickType    xLastWakeTime = xTaskGetTickCount();
    
    HbManager       hb;
    
    bool bDoCtrlAtt;
    bool bDoCtrlMot;
    //
    while(1){
        led1=!led1;
        bDoCtrlAtt = false;
        bDoCtrlMot = false;
        
        // PID係数アップデートチェック
        if(gf_PidParaUpdate){
            hb.setAttPara(g_PidPara);
            sp.printf("Pp  : [%f]\r\n",g_PidPara.PP);
            sp.printf("P   : [%f]\r\n",g_PidPara.P);
            sp.printf("I   : [%f]\r\n",g_PidPara.I);
            sp.printf("D   : [%f]\r\n",g_PidPara.D);
            sp.printf("IMAX: [%f]\r\n",g_PidPara.IMax);
            sp.printf("IMIN: [%f]\r\n",g_PidPara.IMin);
            gf_PidParaUpdate = false;
        }
        for(int i = 0; i < 4; ++i){
            if(gf_MotParaUpdate[i]){
                hb.setMotPara((UCHAR)i, g_MotPara[i]);
                sp.printf("num   : [%d]\r\n",i);
                //sp.printf("ofs  : [%d]\r\n",g_MotPara[i].offset);
                sp.printf("low  : [%d]\r\n",g_MotPara[i].limit_low);
                sp.printf("hi   : [%d]\r\n",g_MotPara[i].limit_hi);
                gf_MotParaUpdate[i] = false;
            }
        }
        if(gf_StopMot){
            setStateF(MOT_STOP);
            hb.getCurMotVal();
            gf_StopMot = false;
        }

        //▼①状態読み出し系
        hb.getAttitude();     //現在角度読み出し
        hb.controlEngine();   //エンジン回転数読み出し
        hb.getUserCommand();  //操作ボタン状態読み出し
        
        //▼②ステート遷移
        switch(gf_State){
            case SLEEP:
                if(gf_Dbg){
                    gf_Dbg = false;
                    taskStart(2);
                }
                else if(gf_Armed){
                    gf_Armed = false;
//                    hb.initChkMotor();
                    setState(WAKEUP);
                }
            break;
            case WAKEUP:
                //各種モーター
                //機材のチェック
//                if(hb.chkMotor() == true){
                    hb.calAtt();
                    // STANDBYでモーター回転をオフセット値まで上げるためのフラグセット
                    //for(int i = 0; i < 4; ++i){
                    //    gf_MtReq[i].bf.req = true;
                    //    gf_MtReq[i].bf.val = 200;
                    //}
                    setState(STANDBY);
//                }
            break;
            case STANDBY:
                // モーター回転数のチェック→モーターの回転数をオフセット値まで上げる
                //nxtStatFlg = true;
                //for(int i = 0; i < 4; ++i){
                //    if(gf_MtReq[i].bf.req){
                //        bDoCtrlMot = true;
                //    }
                //}
                if(!bDoCtrlMot){
                    if(hb.chkInRangeIDLE()){
                        setState(IDLE);
                    }
                    else{
                        // デバッグのためここもスルー
                        setState(IDLE);
                    }
                }
            break;
            case IDLE:
                //SWのチェック
                //離陸ボタン押下
                //とりあえず、YawCtrlで代用
                //if(hb.chkSWUserOpe(HbUserOpe::YAW_CTRL))
                //{
                //    // TODO IMU Cal
                //    hb.calAtt();
                //    setState(TAKE_OFF);
                //}
                if(hb.chkSWUserOpe(HbUserOpe::F_L)){
                    // Front Left 
                    hb.addMotOfs(HbUserOpe::F_L);
                }
                if(hb.chkSWUserOpe(HbUserOpe::F_R)){
                    // Front Right 
                    hb.addMotOfs(HbUserOpe::F_R);
                }
                if(hb.chkSWUserOpe(HbUserOpe::R_L)){
                    // Rear Left 
                    hb.addMotOfs(HbUserOpe::R_L);
                }
                if(hb.chkSWUserOpe(HbUserOpe::R_R)){
                    // Rear Right
                    hb.addMotOfs(HbUserOpe::R_R);
                }
                if(hb.chkSWUserOpeRE(HbUserOpe::EMG_STOP)){
                    // EMG STOP だが、テスト用のため、モーターのオフセットを0にして回転を止める
                    //sp.printf("RisingEdge!\r\n");
                    for(int i = 0; i < 4; ++i){
                        gf_MtReq[i].bf.req = true;
                        gf_MtReq[i].bf.val = 0;
                    }
                }
                
                //else if(hb.chkSWUserOpe(HbUserOpe::ENG_STOP))
                //{
                //   setState(SLEEP);
                //}
                //if(hb.chkSWUserOpeAny()){
                //    sp.printf("SW :[%X]\r\n",hb.getUserSw());
                //}
                bDoCtrlMot = true;
            break;
            case TAKE_OFF:
                // エンジン回転数のチェック
                // 着陸ボタンが優先度が高いので先に判定、たが、まだ作ってないのでコメントアウト
                // if(hb.chkSWUserOpe(HbUserOpe::STOP)){
                //     setState(GROUND);
                // }
                //else
                bDoCtrlMot = true;
                bDoCtrlAtt = true;
                //if(hb.chkSWUserOpe(HbUserOpe::STOP)){
                //    setState(HOVER);
                //}
                
            break;
            case GROUND:
                // そのままスルー
                setState(IDLE);
            break;
            case HOVER:
                // 
            break;
            case DRIVE:
            break;
            case EMGGND:
            break;
            case CHK_ENT:    //チェックエンター
                sp.printf("Check enter\r\n");
            break;
            case CHK_MOT:    //モーターチェック
                sp.printf("Check Motor\r\n");
                bDoCtrlMot = true;
            break;
            case CHK_AXL:    //アクセルサーボチェック
                sp.printf("Check Accel Servo\r\n");
            break;
            case CHK_ATT:    //姿勢制御チェック
                sp.printf("Check Attitude\r\n");
                bDoCtrlAtt = true;
            break;
            case CHK_EXIT:   //チェックステート脱出
                sp.printf("Check Exit\r\n");
            break;
            case MOT_STOP:
                if(hb.stopMotor() == true){
                    hb.initMotVal();
                    setState(SLEEP);
                    sp.printf("MOTOR STOPPED!\r\n");
                }
                else{
                    //bDoCtrlMot = true;
                    sp.printf(".");
                }
            break;
        }
        
        //▼③各種設定
        //hb.controlAttitude(); //姿勢制御
        //hb.controlMotor();//モーター指令出し
        if(bDoCtrlAtt)hb.controlAttitude(); //姿勢制御
        if(bDoCtrlMot)hb.controlMotor();//モーター指令出し
        
        if(gf_Print.bf.stat){
            sp.printf("stat : [%X]\r\n",gf_State);
        }
        //表示フラグを落とす（けどモニタフラグが立ってる箇所は残る）
        if(gf_Print.flg!=0){
            gf_Print.flg=gf_Mon.flg;
            sp.printf("\r\n");
        }
        if(gf_DbgPrint.flg != 0){
            gf_DbgPrint.flg = 0;
            sp.printf("\r\n");
        }
        
        //次の周期まで待つ
        vTaskDelayUntil(&xLastWakeTime, 20 / portTICK_RATE_MS );
    }
}
//========================================================
//タスク２：デバッグ用タスク
//========================================================
void taskDebug(void *pvParameters){

    taskStop(2);//自ら止めてレジュームされるまで待つ
    UCHAR Num;
    INT16 iVal;
    //bool rvFlg;

    //この関数をリターンしてしまうと他のタスクも止まるのでwhileで回すこと！
    while(1){
        for(Num = 0; Num < 8; ++Num){
            for(iVal = 50; iVal < 300; ++iVal){
                fpgaMotor(Num, iVal);
                wait(0.002);
            }
            for(iVal = 300; iVal >= 50; --iVal){
                fpgaMotor(Num, iVal);
                wait(0.002);
            }
        }
        //自タスク停止させて次の周期まで待つ
        gf_Dbg = false;
        sp.printf("Do DebugTask!");
        taskStop(2);
    }
}

//-------------------------------------------------------------
//初期化：タスク登録
//-------------------------------------------------------------
void taskInit(){
    portBASE_TYPE   TaskRtn;//タスククリエイトの結果を受け取る型
    
    //タスク０：コマンド解析タスク
    TaskRtn= xTaskCreate(taskCmdParser, (signed portCHAR *)"TaskCmd", 512, NULL, 1, &tskHandle[0]);
    if(TaskRtn==pdTRUE){sp.printf("Task0 Set : Command parser\r\n");}

    //タスク１：制御タスク
    TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"TaskHbCont", 640, NULL, 2, &tskHandle[1]);
    if(TaskRtn==pdTRUE){sp.printf("Task1 Set : Hoverbike Control\r\n");}
        
    //タスク２：デバッグタスク
    TaskRtn= xTaskCreate(taskDebug, (signed portCHAR *)"TaskDebug", 192, NULL, 0, &tskHandle[2]);
    if(TaskRtn==pdTRUE){sp.printf("Task2 Set : Debug\r\n");}
    
    //RTOSカーネル起動（タスクが走り出す）
    vTaskStartScheduler();
}