File content as of revision 47:d3fa874f336e:

/////////////////////////////////////
/// RTOS関連------------------
#include "FreeRTOS.h"
#include "task.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;
    bool bDoCtrlEng;
    bool bTmp;
    //INT16 tmpRpm, difRpm;
    INT16 cntSW1 = 0;
    INT16 cntSW2 = 0;
    INT16 AxlRpm;
    INT16 TrtlVal;
    float AxlRow;
    //float TrtlRow;
    while(1){
        // DISPLAY LED
        led1=!led1;
        setDO4LED(gf_State);

        hb.proofOfSurvival();
        bDoCtrlAtt = false;
        bDoCtrlMot = false;
        bDoCtrlEng = 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);
            sp.printf("V   : [%f]\r\n",g_PidPara.V);
            gf_PidParaUpdate = false;
        }
        // 確実にモーター等を止めるため
        if(gf_StopMot){
            for(int i = 0; i < 4; ++i){
                gf_MtReq[i].req = true;
                gf_MtReq[i].val = 0;
                gf_MtReqOfs[i].req = true;
                gf_MtReqOfs[i].val = 0;
            }
            for(int i = 0; i < 2; ++i){
                gf_AxReq[i].bf.req = true;
                gf_AxReq[i].bf.val = 0;
            }
            gf_FromActiveStat = isActiveState();
            setStateF(MOT_STOP);
            gf_StopMot = false;
        }


        //▼①状態読み出し系
        hb.getAttitude();     //現在角度読み出し
        hb.getEngine();   //エンジン回転数読み出し
        hb.getUserCommand();  //操作ボタン状態読み出し
       
        //▼②ステート遷移
        switch(gf_State){
            case SLEEP:
                if(gf_StateEnt){
                    sp.printf("SLEEP state\r\n");
                    gf_StateEnt = false;
                }
                // DEBUGタスクスタートフラグの監視
                if(gf_Dbg){
                    gf_Dbg = false;
                    taskStart(2);
                }
                if(hb.chkSWUserOpeAny()){
                    typUserSw sw = hb.getUserSw();
                    //sp.printf("%d%d%d%d%d%d%d%d%d%d\r\n",sw.bf.brk_l,sw.bf.flt_off,sw.bf.r_eng_down,sw.bf.r_eng_up,sw.bf.rsv_1
                    //                ,sw.bf.brk_r,sw.bf.flt_on,sw.bf.f_eng_down,sw.bf.f_eng_up, sw.bf.all_stop);
                } else {
                    //TrtlRow = hb.getUserEngTrottleRaw();
                    if(gf_Print.d2.bf.ain){
                        AxlRow = hb.getUserMotAxlRaw();
                        sp.printf("MotAxl:%f ",AxlRow);
                    }
                }
                // 浮上（離陸）ボタン、着陸ボタンの同時長押し監視
                if(hb.chkSWUserOpeBoth(HbUserOpe::FLT_ON,HbUserOpe::FLT_OFF)){
                    ++cntSW1;
                    if(cntSW1 > 30){
                        cntSW1 = 0;
                        setStateF(CHK_EG_ENT);
                    }
                }
                else {
                    cntSW1 = 0;
                }
                if(hb.chkSWUserOpeBoth(HbUserOpe::BRK_L, HbUserOpe::BRK_R)){
                    ++cntSW2;
                    if(cntSW2 > 30){
                        cntSW2 = 0;
                        setStateF(WAKEUP);
                    }
                }
                else {
                    cntSW2 = 0;
                }
                // Debug用
                bDoCtrlMot = true;
            break;
            case WAKEUP:
                if(gf_StateEnt){
                    //スイッチが何も押されていないことの確認
                    if(!hb.chkSWUserOpeAny()){
                        sp.printf("WAKEUP state\r\n");
                        gf_StateEnt = false;
                    }
                }
                else
                {
                    //各種モーター
                    //機材のチェック
                    hb.calAtt();//現在値でヨー角校正
                    //モーター用のアナログ入力が下げられているかの確認
                    AxlRpm = hb.getUserMotAxl();
                    if(AxlRpm > 0){
                        // 将来的に警告音等
                        sp.printf("Warning!! Motor Accel Opened!![%d]\r\n",AxlRpm);
                        setStateF(SLEEP);
                    } else {
                        setState(STANDBY);
                    }
                }
            break;
            case STANDBY:
                bDoCtrlMot = true;
                if(gf_StateEnt){
                    // モーター回転数のチェック→モーターの回転数をオフセット値まで上げる
                    // TODO
                    sp.printf("STANDBY state\r\n");
                    gf_StateEnt = false;
                }
                else {

                    if(hb.chkInRangeIDLE()){
                        //hb.calAtt();//現在値でヨー角校正
                        setState(IDLE);
                    }
                    else{
                        // デバッグのためここもスルー
                        //hb.calAtt();//現在値でヨー角校正
                        setState(IDLE);
                    }
                }
            break;
            case IDLE:
                if(gf_StateEnt){
                    sp.printf("IDLE state\r\n");
                    gf_StateEnt = false;
                }
                //SWのチェック
                hb.chkSW(IDLE);
                AxlRpm = hb.getUserMotAxl();
                hb.setMotVal(R_L,AxlRpm);
                hb.setMotVal(R_R,AxlRpm);
                // hb.setMotValOfs(R_L,AxlRpm);
                // hb.setMotValOfs(R_R,AxlRpm);
                bDoCtrlEng = true;
                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;
                }
                // エンジン回転数のチェック
                // 着陸ボタンが優先度が高いので先に判定、たが、まだ作ってないのでコメントアウト
                // if(hb.chkSWUserOpe(HbUserOpe::STOP)){
                //     setState(GROUND);
                // }
                //else
                hb.chkSW(TAKE_OFF);
                AxlRpm = hb.getUserMotAxl();
                hb.setMotVal(R_L,AxlRpm);
                hb.setMotVal(R_R,AxlRpm);
                // hb.setMotValOfs(R_L,AxlRpm);
                // hb.setMotValOfs(R_R,AxlRpm);
                bDoCtrlEng = true;
                bDoCtrlMot = true;
                bDoCtrlAtt = true;
            break;
            case GROUND:
                if(gf_StateEnt){
                    sp.printf("GROUND state\r\n");
                    gf_StateEnt = false;
                }
                // そのままスルー
                // TODO エンジン回転数が一定以下になることを確認する
                setState(IDLE);
            break;
            case HOVER:
                gf_StateEnt = false;
                // 
            break;
            case DRIVE:
                gf_StateEnt = false;
            break;
            case EMGGND:
                gf_StateEnt = false;
            break;
            case CHK_EG_ENT:
                if(gf_StateEnt){
                    sp.printf("Check Engine enter\r\n");
                    gf_StateEnt = false;
                }
                if(!hb.chkSWUserOpeAny()){
                    //AxlRpm = hb.getUserMotAxl();
                    TrtlVal = hb.getUserEngTrottle();
                    //if(AxlRpm > 0 || TrtlVal > 40){
                    if(TrtlVal > 10){
                        sp.printf("Throttle Val:%d\r\n",TrtlVal);
                        break;
                    } else {
                        hb.clearHvAxl();
                        setState(CHK_EG_F);
                    }
                }
            break;
            case CHK_EG_F:
                if(gf_StateEnt){
                    sp.printf("Check Engine Front\r\n");
                    gf_StateEnt = false;
                }
                TrtlVal = hb.getUserEngTrottle();
                hb.setAccelVal(FRONT, TrtlVal);
                if(hb.chkSWUserOpe(HbUserOpe::F_ENG_UP)){
                    sp.printf("FRONT Throttle Val:%d\r\n",TrtlVal);
                    hb.setHvAxl(FRONT, TrtlVal);
                }
                if(hb.chkSWUserOpe(HbUserOpe::R_ENG_UP)){
                    if(hb.chkSetHvAxl(FRONT)){
                        gf_AxReq[0].bf.req = true;
                        //gf_AxReq[1].bf.req = true;
                        gf_AxReq[0].bf.val = 0;
                        //gf_AxReq[1].bf.val = 0;
                        setState(CHK_EG_MID);
                    }
                }
                bDoCtrlEng = true;
            break;
            case CHK_EG_MID:
                if(gf_StateEnt){
                    sp.printf("Check Engine MIDDLE\r\n");
                    gf_StateEnt = false;
                }
                if(!hb.chkSWUserOpeAny()){
                    //AxlRpm = hb.getUserMotAxl();
                    TrtlVal = hb.getUserEngTrottle();
                    //if(AxlRpm > 0 || TrtlVal > 40){
                    if(TrtlVal > 10){
                        break;
                    } else {
                        setState(CHK_EG_R);
                    }
                }
                bDoCtrlEng = true;
            break;
            case CHK_EG_R:
                if(gf_StateEnt){
                    sp.printf("Check Engine Rear\r\n");
                    gf_StateEnt = false;
                }
                TrtlVal = hb.getUserEngTrottle();
                hb.setAccelVal(REAR, TrtlVal);
                if(hb.chkSWUserOpe(HbUserOpe::F_ENG_UP)){
                    sp.printf("REAR Throttle Val:%d\r\n",TrtlVal);
                    hb.setHvAxl(REAR, TrtlVal);
                }
                if(hb.chkSWUserOpe(HbUserOpe::R_ENG_UP)){
                    if(hb.chkSetHvAxl(REAR)){
                        //gf_AxReq[0].bf.req = true;
                        gf_AxReq[1].bf.req = true;
                        //gf_AxReq[0].bf.val = 0;
                        gf_AxReq[1].bf.val = 0;
                        setState(CHK_EG_EXIT);
                    }
                }
                bDoCtrlEng = true;
            break;
            case CHK_EG_EXIT:
                if(gf_StateEnt){
                    sp.printf("Check Engine Exit\r\n");
                    gf_StateEnt = false;
                }
                if(!hb.chkSWUserOpeAny()){
                    TrtlVal = hb.getUserEngTrottle();
                    if(TrtlVal > 10){
                        break;
                    } else {
                        setState(SLEEP);
                    }
                }
                bDoCtrlEng = true;
            break;
            case CHK_ENT:    //チェックエンター
                if(gf_StateEnt){
                    sp.printf("Check enter\r\n");
                    gf_StateEnt = false;
                }
                //setStateF(CHK_MOT);
            break;
            case CHK_MOT:    //モーターチェック
                if(gf_StateEnt){
                    sp.printf("Check Motor\r\n");
                    gf_StateEnt = false;
                }
                //AxlRpm = hb.getUserMotAxl();
                bDoCtrlMot = true;
            break;
            case CHK_AXL:    //アクセルサーボチェック
                if(gf_StateEnt){
                    sp.printf("Check Accel Servo\r\n");
                    gf_StateEnt = false;
                }
                //TrtlVal = hb.getUserEngTrottle();
            break;
            case CHK_ATT:    //姿勢制御チェック
                if(gf_StateEnt){
                    sp.printf("Check Attitude\r\n");
                    gf_StateEnt = false;
                }
                bDoCtrlAtt = true;
            break;
            case CHK_EXIT:   //チェックステート脱出
                if(gf_StateEnt){
                    sp.printf("Check Exit\r\n");
                    gf_StateEnt = false;
                }
            break;
            case MOT_STOP:
                if(gf_StateEnt){
                    sp.printf("Motor Stop\r\n");
                    gf_StateEnt = false;
                }
                bDoCtrlAtt = false;
                bDoCtrlMot = true;
                bTmp = true;
                for(int i = 0; i < 4; ++i){
                    if(gf_MtReq[i].req){
                        bTmp = false;
                        break;
                    }
                    if(gf_MtReqOfs[i].req){
                        bTmp = false;
                        break;
                    }
                }
                if(!bTmp)break;
                for(int i = 0; i < 2; ++i){
                    if(gf_AxReq[i].bf.req){
                        bTmp = false;
                        break;
                    }
                }
                if(bTmp){
                    if(gf_FromActiveStat){
                        setStateF(IDLE);
                    }
                    else{
                        setStateF(SLEEP);
                    }
                }

                // 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(bDoCtrlEng)hb.controlEngine(gf_State);//エンジン指令出し

        if(gf_Print.d1.bf.stat){
            sp.printf("stat : [%X] ",gf_State);
        }
        //表示フラグを落とす（けどモニタフラグが立ってる箇所は残る）
        if(gf_Print.d1.flg!=0 || gf_Print.d2.flg!=0){
            gf_Print.d1.flg=gf_Mon.d1.flg;
            gf_Print.d2.flg=gf_Mon.d2.flg;
            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", 1024, 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();
}