File content as of revision 41:45c982b1c5b6:

#include "globalFlags.h"

//モニタ用LED
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

//状態表示用
DigitalOut DO_01(p21);//R
DigitalOut DO_02(p22);//G
DigitalOut DO_03(p23);//B

//モーターアクセル用アナログ入力
AnalogIn AinAxl(p20);

// //エンジンスロットル用アナログ入力
// AnalogIn AinThrottle(p19);

char            g_CmdBuf[G_CMD_BUF_SIZ] ;// コマンド受け渡しバッファ
bool            gf_CmdPrs;

bool            gf_Armed            ;//アーミングフラグ
bool            gf_Dbg              ;//デバッグタスク起動フラグ
//bool            gf_fs_stat          ;//強制遷移フラグ
bool            gf_StopMot          ;//モーターの強制停止 サーボ全閉
bool            gf_FromActiveStat   ;//モーターの強制停止 サーボ全閉が動作時に発生したかどうか

typPrintFlg    gf_Print            ;//
typPrintFlg    gf_Mon              ;//
typCalFlag      gf_Cal              ;//
//typDbgPrintFlg  gf_DbgPrint         ;//デバッグ用
typAccel        gf_AxReq[2]           ;//アクセル更新フラグ
typAxlRpm        gf_MtReq[4]              ;//モーター姿勢制御更新フラグ
typAxlRpm        gf_MtReqOfs[4]           ;//モーターオフセット更新フラグ
typAxlRpm        gf_MtReqDct[8]           ;//ダイレクト(FPGA関数直接呼び出し)モーター更新フラグ
typAxlRpm        gf_MtReqU[4]           ;//モーターユーザー更新フラグ

enmHbState      gf_State = SLEEP;
bool            gf_StateEnt             ;//状態遷移後、最初であることを示す

bool            gf_PidParaUpdate       ;//PID Pp,P,I,Dの係数アップデートフラグ
typPidPara      g_PidPara       ;//PID Pp,P,I,Dの係数の外部設定用

// bool            gf_MotParaUpdate[4]        ;//モーターのパラメータ値更新フラグ
// typMotPara      g_MotPara[4];

void initFlags(){
    memset(g_CmdBuf,0x0,sizeof(g_CmdBuf));
    gf_CmdPrs = false;
    gf_Armed = false;
    gf_Dbg = false;
    gf_StopMot = false;

//    gf_Print.flg = 0;
//    gf_Mon.flg = 0;
    gf_Print.d1.flg = 0;
    gf_Print.d2.flg = 0;

    gf_Cal.flg = 0;

    //gf_DbgPrint.flg = 0;

    gf_AxReq[0].dt = 0;
    gf_AxReq[1].dt = 0;
    for(int i=0; i<4;++i){
        gf_MtReq[i].req = false;
        gf_MtReq[i].val = 0;
        gf_MtReqOfs[i].req = false;
        gf_MtReqOfs[i].val = 0;
        gf_MtReqU[i].req = false;
        gf_MtReqU[i].val = 0;
    }
    for(int i = 0; i < 8; ++i){
        gf_MtReqDct[i].req = false;
        gf_MtReqDct[i].val = 0;
    }
    gf_State = SLEEP;
    gf_StateEnt = false;
    g_PidPara.PP = 2;
    g_PidPara.P = 0;
    g_PidPara.I = 0;
    g_PidPara.D = 0;
    g_PidPara.IMax = 2000;
    g_PidPara.IMin = -2000;
    // for(int i = 0; i < 4; ++i){
    //     gf_MotParaUpdate[i] = false;
    //     g_MotPara[i].limit_hi = LIM_MOT_INP_MAX;
    //     g_MotPara[i].limit_low = LIM_MOT_INP_MIN;
    // }
}

void setDOCol(eLedCol col)
{
    switch(col){
        case WHITE:
            DO_01 = !DO_01;
            DO_02 = !DO_02;
            DO_03 = !DO_03;
        break;
        case RED:
            DO_01 = !DO_01;
            DO_02 = 0;
            DO_03 = 0;
        break;
        case GREEN:
            DO_01 = 0;
            DO_02 = !DO_02;
            DO_03 = 0;
        break;
        case BLUE:
            DO_01 = 0;
            DO_02 = 0;
            DO_03 = !DO_03;
        break;
        case YELLOW:
            DO_01 = !DO_01;
            DO_02 = !DO_02;
            DO_03 = 0;
        break;
        case PURPLE:
            DO_01 = !DO_01;
            DO_02 = 0;
            DO_03 = !DO_03;
        break;
        case LIGHT_BLUE:
            DO_01 = 0;
            DO_02 = !DO_02;
            DO_03 = !DO_03;
        break;
        case OFF:
        default:
            DO_01 = 0;//R
            DO_02 = 0;//G
            DO_03 = 0;//B
        break;
    }
}

void setDO4LED(enmHbState stat)
{
    if(stat == NONE)
    {// 消灯
        setDOCol(OFF);
    }
    else if(stat == SLEEP)
    {//YELLOW
        setDOCol(YELLOW);
    }
    else if(stat == WAKEUP || stat == STANDBY)
    {//BLUE
        setDOCol(BLUE);
    }
    else if(stat == IDLE)
    {//GREEN
        setDOCol(GREEN);
    }
    else if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE || stat == GROUND || stat == EMGGND)
    {//PURPLE
        setDOCol(PURPLE);
    }
    else if(stat == CHK_EG_ENT || stat == CHK_EG_F || stat == CHK_EG_MID || stat == CHK_EG_R || stat == CHK_EG_EXIT)
    {//LIGHT BLUE
        setDOCol(LIGHT_BLUE);
    }
    else if(stat == CHK_ENT || stat == CHK_MOT || stat == CHK_AXL || stat == CHK_ATT || stat == CHK_EXIT)
    {//WHITE
        setDOCol(WHITE);
    }
    else/* if(stat == MOT_STOP)*/
    {
        setDOCol(RED);
    }
}

void setState(enmHbState stat){
    // ありえない遷移を排除
    if(gf_State == SLEEP){if(stat != WAKEUP) return;}
    if(gf_State == WAKEUP){if(stat != STANDBY) return;}
    if(gf_State == STANDBY){if(stat !=IDLE) return;}
    if(gf_State == IDLE){if(stat != TAKE_OFF && stat != SLEEP) return;}
    if(gf_State == TAKE_OFF){if(stat != HOVER && stat != GROUND) return;}
    if(gf_State == GROUND){if(stat != IDLE) return;}
    if(gf_State == HOVER){if(stat != DRIVE && stat != GROUND) return;}
    if(gf_State == DRIVE){if(stat != HOVER && stat != EMGGND) return;}
    if(gf_State == EMGGND){if(stat != IDLE) return;}

    sp.printf("state: %d\r\n",stat);
    gf_State = stat;
    gf_StateEnt = true;
}
void setStateF(enmHbState stat){
    sp.printf("state: %d\r\n",stat);
    enmHbState tmp;
    if((UINT16)stat > MOT_STOP){
        tmp = SLEEP;
    }
    else
    {
        tmp = stat;
    }
    gf_State = tmp;
    gf_StateEnt = true;
}

bool isActiveState(){
    if(gf_State  >= IDLE && gf_State <= EMGGND){
        return true;
    }
    else {
        return false;
    }
}

/*
//===========================================
//コンストラクタ
//===========================================
GlobalFlags::GlobalFlags(){
    //初期ステート
    state   = SLEEP;
    //ステートキュー初期化
    for(int i=0; i<STATE_QUE_SIZ; i++){
      stateQueue[i] = NONE;
    }
    rp = wp = 0;//先頭を指す
    full = false;//最初はエンプティ
    empty = true;
}


//-----------------------------------
//ステートのFIFOにプッシュする
//-----------------------------------
bool GlobalFlags::push(enmHbState iState){
    if(full){
        return false;//FFULLフラグが立ってら入力を受け付けない
    }else{
        stateQueue[wp] = iState;//キューに突っ込んで
        wp    = (wp==STATE_QUE_SIZ) ? 0 : wp+1;//ライトポインタを進める
        full  = (wp == rp)? true : false; //次ライト予定の場所がリードポインタに追いついたらFULLを立てる
        empty = false;
        return true;
    }
}

//-----------------------------------
//ステートFIFOから次の予約ステートを引き抜く
//-----------------------------------
bool GlobalFlags::pull(){
    if(empty){
        return false;//EMPTYだったら引き抜けない
    }else{
        state = stateQueue[rp];//リードポインタの指すデータをステートに移す
        stateQueue[rp] = NONE;//バッファを消す
        rp = (rp==STATE_QUE_SIZ) ? 0 : rp+1;//リードポインタを進める
        full  = false; //フルではなくなる
        empty = (stateQueue[rp] == NONE) ? true : false;//次にリードする場所がNONEだったらEMPTYを立てる
        return true;
    }
}
*/