File content as of revision 24:c5945aaae777:

#include "globalFlags.h"
//#define STATE_BUF_SIZ 8

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

//GlobalFlags     gf;

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

bool            gf_Armed            ;//アーミングフラグ
bool            gf_Dbg              ;//デバッグタスク起動フラグ
bool            gf_fs_stat          ;//強制遷移フラグ 
typPrintFlag    gf_Print            ;//
typPrintFlag    gf_Mon              ;//
typCalFlag      gf_Cal              ;//
//typAccel        gf_AxReq[2] ={0,}   ;//アクセル更新
//typAccel        gf_MtReq[4] ={0,}   ;//モーター更新
typAccel        gf_AxReq[2]           ;//アクセル更新フラグ
typAccel        gf_MtReq[4]           ;//モーター更新フラグ

enmHbState      gf_State = SLEEP;

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

void initFlags(){
    memset(g_CmdBuf,0x0,sizeof(g_CmdBuf));
    gf_CmdPrs = false;
    gf_Armed = false;
    gf_Dbg = false;
    gf_Print.flg = 0;
    gf_Mon.flg = 0;
    gf_Cal.flg = 0;
    gf_AxReq[0].dt = 0;
    gf_AxReq[1].dt = 0;
    gf_MtReq[0].dt = 0;
    gf_MtReq[1].dt = 0;
    gf_MtReq[2].dt = 0;
    gf_MtReq[3].dt = 0;
    gf_State = SLEEP;
    g_PidPara.PP = 4;
    g_PidPara.P = 4;
    g_PidPara.I = 0.02;
    g_PidPara.D = 0;
    g_PidPara.IMax = 2000;
    g_PidPara.IMin = -2000;
}

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;
}
void setStateF(enmHbState stat){
    sp.printf("state: %d\r\n",stat);
    enmHbState tmp;
    if((UINT16)stat > CHK_EXIT){
        tmp = SLEEP;
    }
    else
    {
        tmp = stat;
    }
    gf_State = tmp;
}
/*
//===========================================
//コンストラクタ
//===========================================
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;
    }
}
*/