File content as of revision 67:ad35921e3cdc:

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

UINT16 HbManager::proofOfSurvival()
{
    return fpgaTest(1);
}

//------------------------------------------------------
//姿勢状態取得
//------------------------------------------------------
void HbManager::getAttitude(){
    float tmpf;
    INT16 tmpi;
    //姿勢状態取得
    nowAngle = imu->GetYaw();
    nowRate  = imu->GetGyroZ();

    if(gf_Cal.bf.yaw==true && gf_Cal.bf.gy==true){
        nowTrgtAng = 0;
    }
    //ヨー角キャリブレーション   
    if(gf_Cal.bf.yaw==true){
        imu->CalYaw();
        gf_Cal.bf.yaw=false;
        sp.printf("Yaw caribration %f\t",nowAngle);
        nowAngle=0;
    }
    //ジャイロオフセットキャリブレーション   
    if(gf_Cal.bf.gy==true){
        imu->CalGyro();
        gf_Cal.bf.gy=false;
        sp.printf("Gyro caribration %d\t",nowRate);
        nowRate=0;
    }
    
    if(gf_Print.d1.bf.yaw) {
        tmpf = imu->GetYawRef();
        sp.printf("Ang\t%f ref %f\t",nowAngle,tmpf);
    }
    if(gf_Print.d1.bf.gy)  {
        tmpi = imu->GetGyroRef();
        sp.printf("Gyr\t%d ref %d\t",nowRate,tmpi);
    }
    for(int i = 0; i < 4; ++i){
        if(gf_MtAttOfs[i].req){
            motorOfsVal[i] = gf_MtAttOfs[i].val;
            gf_MtAttOfs[i].req = false;
        }
    }
    if(gf_MtBrk.req){
        mot_brk = gf_MtBrk.val;
        gf_MtBrk.req = false;
        sp.printf("Mot Brk:[%d]",mot_brk);
    }
    if(gf_AngBrk.req){
        ang_rate_brk = gf_AngBrk.val / static_cast<float>(UPDATE_RATE);
        gf_AngBrk.req = false;
        sp.printf("Ang Rate Brk:[%f]",ang_rate_brk);
    }
}
void HbManager::calAtt(){
    if(att == NULL) return;
    if(imu == NULL) return;
    imu->CalYaw();
    sp.printf("Yaw auto caribration %f\t",nowAngle);
    nowAngle=0;
    imu->CalGyro();
    sp.printf("Gyro auto caribration %d\t\r\n",nowRate);
    nowRate=0;

    nowTrgtAng = 0;
}

//------------------------------------------------------
//姿勢制御
//------------------------------------------------------
void HbManager::controlAttitude(){

    float pidRtn;
    
    if(g_PidPara.mode == PID_0_OFF || g_PidPara.mode == PID_0_ON){
        pidRtn = att->pid(nowTrgtAng,nowAngle,nowRate);
    } else {
        pidRtn = att->pid2(nowTrgtAng,nowAngle,nowRate);
    }

    if(att != NULL){
        if(gf_Print.d1.bf.pp){sp.printf("PID:PP=%f,",att->getPp());}
        if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
        if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
        if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
        if(gf_Print.d1.bf.v) {sp.printf("PID:V=%f,",att->getV()); }
        if(gf_Print.d1.bf.fb){
            sp.printf("PID FB: %f, %f, %f, %f,", pidRtn, nowTrgtAng, nowAngle, nowRate);
        }
    }
    float fl, fr, bl, br;
    if(gf_State == TAKE_OFF || gf_State == DRIVE || gf_State == HOVER){
        if(g_PidPara.mode == PID_0_ON){
            if(pidRtn < 0){
                //fl = pidRtn - motorOfsVal[0];
                fl = 0.0;
                fr = -pidRtn + motorOfsVal[1];
                bl = -pidRtn + motorOfsVal[2];
                //br = pidRtn - motorOfsVal[3];
                br = 0.0;
            } else {
                fl = pidRtn + motorOfsVal[0];
                //fr = -pidRtn - motorOfsVal[1];
                //bl = -pidRtn - motorOfsVal[2];
                fr = 0.0;
                bl = 0.0;
                br = pidRtn + motorOfsVal[3];
            }
        } else if(g_PidPara.mode == PID_1_ON){
            if(pidRtn < 0){
                fl = -pidRtn * 2.25 + motorOfsVal[0];
                fr = pidRtn * 2.25 - motorOfsVal[1];
                bl = pidRtn * 2.25 - motorOfsVal[2];
                br = -pidRtn * 2.25 + motorOfsVal[3];
            } else {
                fl = -pidRtn * 2.25 - motorOfsVal[0];
                fr = pidRtn * 2.25 + motorOfsVal[1];
                bl = pidRtn * 2.25 + motorOfsVal[2];
                br = -pidRtn * 2.25 - motorOfsVal[3];
            }
        }else{
            fl = 0;
            fr = 0;
            bl = 0;
            br = 0;
        }
    } else {
        fl = 0;
        fr = 0;
        bl = 0;
        br = 0;
    }
    gf_MtReq[0].val = (INT16)fl;
    gf_MtReq[1].val = (INT16)fr;
    gf_MtReq[2].val = (INT16)bl;
    gf_MtReq[3].val = (INT16)br;
    for(int i = 0; i < 4; ++i){
        gf_MtReq[i].req = true;
    }
}

void HbManager::controlAttitude(float cmdAngle){
    float pidRtn;

    pidRtn = att->pid2(cmdAngle,nowAngle,nowRate);

    if(gf_Print.d1.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
    if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
    if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
    if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
    if(gf_Print.d1.bf.v) {sp.printf("PID:V=%f,",att->getV()); }
    if(gf_Print.d1.bf.fb){
        sp.printf("PID FB,%f,",pidRtn);
        sp.printf("%f,",nowAngle);
        sp.printf("%f,",nowRate);
        }
    
    //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
    // MAX RPM=9000 PWM MAX Val=4095 -> 2.25
    float fl = pidRtn * 2.25 * -1;
    float fr = pidRtn * 2.25;
    float bl = pidRtn * 2.25;
    float br = pidRtn * 2.25 * -1;
    gf_MtReq[0].val = (INT16)fl;
    gf_MtReq[1].val = (INT16)fr;
    gf_MtReq[2].val = (INT16)bl;
    gf_MtReq[3].val = (INT16)br;
    for(int i = 0; i < 4; ++i){
        gf_MtReq[i].req = true;
    }

    // float pidRtn;

    // pidRtn= att->pid(cmdAngle,nowAngle,nowRate);
    // //
    // if(gf_Print.d1.bf.pp){sp.printf("PID:P=%f,",att->getPp());}
    // if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); }
    // if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); }
    // if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); }
    // if(gf_Print.d1.bf.fb){sp.printf("PID feedback=%f,",pidRtn);}
    
    // //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
    // float fl = pidRtn * -1;
    // float fr = pidRtn;
    // float bl = pidRtn;
    // float br = pidRtn * -1;
    
    // motorVal[0] = (INT16)fl;//前左
    // motorVal[1] = (INT16)fr;//前右
    // motorVal[2] = (INT16)bl;//後左
    // motorVal[3] = (INT16)br;//後右
}

void HbManager::setAttPara(typPidPara para){
    if(att == NULL) return;
    att->setPp(para.PP);
    att->setP(para.P);
    att->setI(para.I);
    att->setD(para.D);
    att->setIMax(para.IMax);
    att->setIMin(para.IMin);
    att->setV(para.V);
}

//------------------------------------------------------
//モーター制御
//------------------------------------------------------
void HbManager::controlMotor(){
    INT16 tmp,dif;
    // DEBUG用FPGAに直接指示用
    INT16 pos;
    INT16 typ;
    if(gf_State == SLEEP){
        for(int i = 0; i < 8; ++i){
            pos = i / 2;
            typ = i % 2;
            tmp = subProp[pos]->getValueFpgaMot((eMotType)typ);
            if(gf_MtReqDct[i].req){
                dif = tmp - (INT16)gf_MtReqDct[i].val;
                if(dif == 0){
                    subProp[pos]->setValueFpgaMot((eMotType)typ, (INT16)gf_MtReqDct[i].val);
                    gf_MtReqDct[i].req = false;
                }
                else {
                    if(1000 < dif){
                        tmp-=100;
                    }
                    else if(100 < dif){
                        tmp-= 10;
                    }
                    else if(100 >= dif && 10 < dif){
                        tmp-= 5;
                    }
                    else if ( 10 >= dif && 1 <= dif){
                        tmp-=1;
                    }
                    else if ( -1000 > dif){
                        tmp+=100;
                    }
                    else if ( -100 >= dif){
                        tmp+=10;
                    }
                    else if(-100 <= dif && -10 > dif){
                        tmp+= 5;
                    }
                    else if ( -10 <= dif && -1 >= dif){
                        tmp+=1;
                    }
                    subProp[pos]->setValueFpgaMot((eMotType)typ, tmp);
                }
            }
            else
            {
                subProp[pos]->setValueFpgaMot((eMotType)typ, tmp);
            }
        }
    }
    else
    {
        // オフセット分の回転数セット
        for(int i = 0; i < 4; ++i){
            tmp = subProp[i]->getValue(OFS);
            if(gf_MtReqOfs[i].req){
                dif = tmp - (INT16)gf_MtReqOfs[i].val;
                if(dif == 0){
                    subProp[i]->setValue(OFS, (INT16)gf_MtReqOfs[i].val);
                    gf_MtReqOfs[i].req = false;
                }
                else {
                    if(1000 < dif){
                        tmp-=100;
                    }
                    else if(100 < dif){
                        tmp-= 10;
                    }
                    else if(100 >= dif && 10 < dif){
                        tmp-= 5;
                    }
                    else if ( 10 >= dif && 1 <= dif){
                        tmp-=1;
                    }
                    else if ( -1000 > dif){
                        tmp+=100;
                    }
                    else if ( -100 >= dif){
                        tmp+=10;
                    }
                    else if(-100 <= dif && -10 > dif){
                        tmp+= 5;
                    }
                    else if ( -10 <= dif && -1 >= dif){
                        tmp+=1;
                    }
                    subProp[i]->setValue(OFS, tmp);
                }
            }
            else
            {
                subProp[i]->setValue(OFS, tmp);
            }
        }

        // 姿勢制御分の回転数セット
        for(int i = 0; i< 4;++i){
            if(gf_MtReq[i].req){
                subProp[i]->setValue(ATT,gf_MtReq[i].val);
                gf_MtReq[i].req = false;
            }
            else
            {
                tmp = subProp[i]->getValue(ATT);
                subProp[i]->setValue(ATT,tmp);
            }
        }

        // ユーザー操作分の回転数セット
        for(int i = 0; i < 4; ++i){
            if(gf_MtReqU[i].req){
                subProp[i]->setValue(USER,gf_MtReqU[i].val);
                gf_MtReqU[i].req = false;
            }
            else
            {
                tmp = subProp[i]->getValue(USER);
                subProp[i]->setValue(USER,tmp);
            }
        }

        for(int i = 0; i < 4; ++i){
            subProp[i]->setValue();
        }
    }

    if(gf_Print.d1.bf.m1){ sp.printf("m1=%d,",(INT16)subProp[0]->getValue(ATT)); }
    if(gf_Print.d1.bf.m2){ sp.printf("m2=%d,",(INT16)subProp[1]->getValue(ATT)); }
    if(gf_Print.d1.bf.m3){ sp.printf("m3=%d,",(INT16)subProp[2]->getValue(ATT)); }
    if(gf_Print.d1.bf.m4){ sp.printf("m4=%d,",(INT16)subProp[3]->getValue(ATT)); }
}

INT16 HbManager::getCurMotVal(eMotPos pos){
    if(subProp[(int)pos] == NULL) return 0;
    return subProp[(int)pos]->getValue(USER);
}

void HbManager::setMotVal(eMotPos pos, INT16 val){
    if(subProp[(int)pos] == NULL) return;
    subProp[(int)pos]->setValue(USER, val);
}

void HbManager::setMotValOfs(eMotPos pos, INT16 val){
    if(subProp[(int)pos] == NULL) return;
    subProp[(int)pos]->setValue(OFS, val);
}

void HbManager::setMotValAtt(eMotPos pos, INT16 val){
    if(subProp[(int)pos] == NULL) return;
    subProp[(int)pos]->setValue(ATT, val);
}

void HbManager::setFpgaMot(eMotPos pos,eMotType type, INT16 val){
    if(subProp[(int)pos] == NULL) return;
    subProp[(int)pos]->setValueFpgaMot(type, val);
}

// void HbManager::setMotFPGA(UCHAR num, INT16 val){
//     //UCHAR tmpN = num / 2;
//     //UCHAR tmp = num % 2;
// //    mot[tmpN]->setValueFPGA(tmp, val);
// //    sp.printf("FPGA Direct NUM:[%d] [%d]\r\n",tmpN, tmp, val);
// }

//------------------------------------------------------
//エンジン制御
//------------------------------------------------------
void HbManager::getEngine(){
    UINT16  rpm[2];
    
    //エンジン回転数読み出し
    rpm[0] = eng[0]->getRpm();
    rpm[1] = eng[1]->getRpm();
    
    if(gf_Print.d1.bf.e1){ sp.printf("%d",rpm[0]); }
    if(gf_Print.d1.bf.e2){ sp.printf("%d,",rpm[1]); }
    if(gf_Print.d2.bf.ep1){sp.printf("set %d hover %d ", getAccelVal(FRONT), getHvAxl(FRONT));}
    if(gf_Print.d2.bf.ep2){sp.printf("set %d hover %d ", getAccelVal(REAR), getHvAxl(REAR));}

    if(gf_AxReqH[0].bf.req){
        setHvAxl(FRONT,gf_AxReqH[0].bf.val);
        gf_AxReqH[0].bf.req = false;
    }
    if(gf_AxReqH[1].bf.req){
        setHvAxl(REAR,gf_AxReqH[1].bf.val);
        gf_AxReqH[1].bf.req = false;
    }
    for(int i = 0; i < 2; ++i){
        if(gf_AxStepReq[i].bf.req){
            en_srv_step[i] = gf_AxStepReq[i].bf.val;
            gf_AxStepReq[i].bf.req = false;
            sp.printf("set Axl Step [%d] val[%d]",i+1, en_srv_step[i]);
        }
        if(gf_AxAdjStepReq[i].bf.req){
            en_srv_adj_step[i] = gf_AxAdjStepReq[i].bf.val;
            gf_AxAdjStepReq[i].bf.req = false;
            sp.printf("set Axl Adjust Step [%d] val[%d]",i+1, en_srv_adj_step[i]);
        }
    }
}

void HbManager::controlEngine(){
    int     i;
    INT16 tmp, dif;
    INT16 step = 100;
    //アクセル設定
    for(i=0; i<2; i++){
        if(gf_AxReq[i].bf.req==true){
            step = en_srv_step[i];
            tmp = eng[i]->getAccel();
            dif = tmp - (INT16)gf_AxReq[i].bf.val;
            if(dif == 0){
                accelVal[i] = gf_AxReq[i].bf.val;
                eng[i]->setAccel(accelVal[i]);
                gf_AxReq[i].bf.req=false;
                //sp.printf("Servo Moving FINISH!!\r\n");
            }
            else {
                if(dif > 0){
                    if(dif > step){
                        tmp -= step;
                    }
                    else 
                    {
                        tmp -= dif;
                    }
                }
                else {
                    if( abs(dif) > step){
                        tmp += step;
                    }
                    else
                    {
                        tmp -= dif;
                    }
                }
                eng[i]->setAccel(tmp);
            }
            //sp.printf("val=%d\r\n" , gf_AxReq[i].bf.val);
        }
        else
        {// 直前でセットされた値をセット
            eng[i]->setAccel(accelVal[i]);
        }
    }
}

void HbManager::controlEngine(enmHbState stat){
    int     i;
    INT16 tmp, dif;
    INT16 step = 100;
    //アクセル設定
    if(stat == CHK_EG_F || stat == CHK_EG_R){
        for(i=0; i<2; i++){
            if(gf_AxReq[i].bf.req==true){
                accelVal[i] = gf_AxReq[i].bf.val;
                eng[i]->setAccel(accelVal[i]);
                gf_AxReq[i].bf.req=false;
                //sp.printf("val=%d\r\n" , accelVal[i]);
            }
            else
            {// 直前でセットされた値をセット
                eng[i]->setAccel(accelVal[i]);
            }
        }
    }else {
        for(i=0; i<2; i++){
            if(gf_AxReq[i].bf.req==true){
                step = en_srv_step[i];
                tmp = eng[i]->getAccel();
                dif = tmp - (INT16)gf_AxReq[i].bf.val;
                if(dif == 0){
                    accelVal[i] = gf_AxReq[i].bf.val;
                    eng[i]->setAccel(accelVal[i]);
                    gf_AxReq[i].bf.req=false;
                    //sp.printf("Servo Moving FINISH!!\r\n");
                }
                else {
                    if(dif > 0){
                        if(dif > step){
                            tmp -= step;
                        }
                        else 
                        {
                            tmp -= dif;
                        }
                    }
                    else {
                        if( abs(dif) > step){
                            tmp += step;
                        }
                        else
                        {
                            tmp -= dif;
                        }
                    }
                    eng[i]->setAccel(tmp);
                }
                //sp.printf("val=%d\r\n" , gf_AxReq[i].bf.val);
            }
            else
            {// 直前でセットされた値をセット
                eng[i]->setAccel(accelVal[i]);
            }
        }
    }
}

void HbManager::clearHvAxl(){
    for(int i = 0; i < 2; ++i){
        eng[i]->clearHoverAccel();
    }
}
bool HbManager::chkSetHvAxl(eEgPos pos){
    if(pos == FRONT){
        return eng[0]->chkSetHoverAccel();
    }
     else/* if(pos == REAR)*/{
        return eng[1]->chkSetHoverAccel();
    }
}
void HbManager::setHvAxl(eEgPos pos, INT16 val){
    if(pos == FRONT){
        eng[0]->setHoverAccel(val);
    }
    else/* if(pos == REAR)*/{
        eng[1]->setHoverAccel(val);
    }
}
INT16 HbManager::getHvAxl(eEgPos pos){
    if(pos == FRONT){
        return eng[0]->getHoverAccelVal();
    }
    else/* if(pos == REAR)*/{
        return eng[1]->getHoverAccelVal();
    }
}
void HbManager::setAccelVal(eEgPos pos, INT16 val){
    if(pos == FRONT){
        gf_AxReq[0].bf.req = true;
        gf_AxReq[0].bf.val = val > MAX_VAL_12BIT ? MAX_VAL_12BIT: (UINT16)val;
    } else {
        gf_AxReq[1].bf.req = true;
        gf_AxReq[1].bf.val = val > MAX_VAL_12BIT ? MAX_VAL_12BIT: (UINT16)val;
    }
}
INT16 HbManager::getAccelVal(eEgPos pos){
    if(pos == FRONT){
        return eng[0]->getAccel();
    }
    else/* if(pos == REAR)*/{
        return eng[1]->getAccel();
    }
}
//------------------------------------------------------
//ステート遷移関連
//------------------------------------------------------
bool HbManager::chkOverIDLE(){
    return eng[0]->chkOverIDLECycle() && eng[1]->chkOverIDLECycle();
}

bool HbManager::chkInRangeIDLE(){
    return eng[0]->chkInRangeIDLE() && eng[1]->chkInRangeIDLE();
}
// ステート遷移関連end
//------------------------------------------------------

//------------------------------------------------------
//ユーザー操作関連
//------------------------------------------------------
void HbManager::getUserCommand(){
    usrSW = ope->GetUserOpe();
    //sp.printf("SW Val=%X\r\n", usrSW.w);
}

float HbManager::getUserMotAxlRaw(){
    return ope->GetAinAccelRaw();
}

INT16 HbManager::getUserMotAxl(){
    return ope->GetAinAccel();
}

float HbManager::getUserEngTrottleRaw(){
    return ope->GetAinThrottleRaw();
}

INT16 HbManager::getUserEngTrottle(){
    return ope->GetAinThrottle();
}

bool HbManager::chkSWUserOpe(HbUserOpe::SW_TYPE stype){
    return ope->ChkCtrlSW(stype);
}
bool HbManager::chkSWUserOpeRE(HbUserOpe::SW_TYPE stype){
    return ope->ChkCtrlSwRiseEdge(stype);
}
bool HbManager::chkSWUserOpeAny(){
    return ope->ChkCtrlSwAny();
}

bool HbManager::chkSWUserOpeBoth(HbUserOpe::SW_TYPE stype1,HbUserOpe::SW_TYPE stype2){
    return ope->ChkCtrlSwBoth(stype1,stype2);
}

typUserSw HbManager::getUserSw(){
    return usrSW;
}

void HbManager::chkSW(enmHbState stat){
    
    //INT16 tmpRpm;
    INT16 tmpAxl;
        // Front Left 
    if(g_PidPara.mode == PID_0_ON){
        if(chkSWUserOpeRE(HbUserOpe::BRK_L) || chkSWUserOpe(HbUserOpe::BRK_L)){
            nowTrgtAng -= ang_rate_brk;
        }
        if(chkSWUserOpeRE(HbUserOpe::BRK_R) || chkSWUserOpe(HbUserOpe::BRK_R)){
            nowTrgtAng += ang_rate_brk;
        }
        for(int i = 0; i < 4; ++i){
            gf_MtReqU[i].req = true;
            gf_MtReqU[i].val = 0;
        }
    }
    else{
        if(chkSWUserOpeRE(HbUserOpe::BRK_L)){
            //nowTrgtAng -= 1;
            gf_MtReqU[0].req = true;
            gf_MtReqU[0].val = mot_brk;
            //setMotVal(F_L, mot_brk);
            gf_MtReqU[3].req = true;
            gf_MtReqU[3].val = mot_brk;
            //setMotVal(R_R, mot_brk);
        } else if(chkSWUserOpe(HbUserOpe::BRK_L)){
            //nowTrgtAng -= 0.1;
            gf_MtReqU[0].req = true;
            gf_MtReqU[0].val = mot_brk;
            //setMotVal(F_L, mot_brk);
            gf_MtReqU[3].req = true;
            gf_MtReqU[3].val = mot_brk;
            //setMotVal(R_R, mot_brk);
        }else if(!chkSWUserOpe(HbUserOpe::BRK_L)){
            gf_MtReqU[0].req = true;
            gf_MtReqU[0].val = 0;
            //setMotVal(F_L, mot_brk);
            gf_MtReqU[3].req = true;
            gf_MtReqU[3].val = 0;
        }
    // else if(!chkSWUserOpe(HbUserOpe::BRK_L)){
    //     if(gf_MtReqU[0].req){
    //         tmpRpm = getCurMotVal(F_L);
    //         if(tmpRpm == 0){
    //             gf_MtReqU[0].req = false;
    //         }else if(tmpRpm > 1000){
    //             tmpRpm-=100;
    //         } else if(tmpRpm >= 100){
    //             tmpRpm-=50;
    //         } else if(tmpRpm > 0){
    //             tmpRpm-=10;
    //             if(tmpRpm < 0) tmpRpm = 0;
    //         } else if(tmpRpm < -1000){
    //             tmpRpm+=100;
    //         } else if(tmpRpm <= -100){
    //             tmpRpm+=50;
    //         }else if(tmpRpm < 0){
    //             tmpRpm+=10;
    //             if(tmpRpm > 0) tmpRpm = 0;
    //         }
    //         setMotVal(F_L, tmpRpm);
    //     }
    //     if(gf_MtReqU[3].req){
    //         tmpRpm = getCurMotVal(R_R);
    //         if(tmpRpm == 0){
    //             gf_MtReqU[3].req = false;
    //         }else if(tmpRpm > 1000){
    //             tmpRpm-=100;
    //         } else if(tmpRpm >= 100){
    //             tmpRpm-=50;
    //         } else if(tmpRpm > 0){
    //             tmpRpm-=10;
    //             if(tmpRpm < 0) tmpRpm = 0;
    //         } else if(tmpRpm < -1000){
    //             tmpRpm+=100;
    //         } else if(tmpRpm <= -100){
    //             tmpRpm+=50;
    //         }else if(tmpRpm < 0){
    //             tmpRpm+=10;
    //             if(tmpRpm > 0) tmpRpm = 0;
    //         }
    //         setMotVal(R_R, tmpRpm);
    //     }
    // }

        if(chkSWUserOpeRE(HbUserOpe::BRK_R)){
            //nowTrgtAng += 1;
            gf_MtReqU[1].req = true;
            gf_MtReqU[1].val = mot_brk;
            //setMotVal(F_R, mot_brk);
            gf_MtReqU[2].req = true;
            gf_MtReqU[2].val = mot_brk;
            //setMotVal(R_L, mot_brk);
        } else if(chkSWUserOpe(HbUserOpe::BRK_R)){
            //nowTrgtAng += 0.1;
            gf_MtReqU[1].req = true;
            gf_MtReqU[1].val = mot_brk;
            //setMotVal(F_R, mot_brk);
            gf_MtReqU[2].req = true;
            gf_MtReqU[2].val = mot_brk;
            //setMotVal(R_L, mot_brk);
        } else if(!chkSWUserOpe(HbUserOpe::BRK_R)){
            gf_MtReqU[1].req = true;
            gf_MtReqU[1].val = 0;
            gf_MtReqU[2].req = true;
            gf_MtReqU[2].val = 0;
        }
    // else if(!chkSWUserOpe(HbUserOpe::BRK_R)){
    //     if(gf_MtReqU[1].req){
    //         tmpRpm = getCurMotVal(F_R);
    //         if(tmpRpm == 0){
    //             gf_MtReqU[1].req = false;
    //         }else if(tmpRpm > 1000){
    //             tmpRpm-=100;
    //         } else if(tmpRpm >= 100){
    //             tmpRpm-=50;
    //         } else if(tmpRpm > 0){
    //             tmpRpm-=10;
    //             if(tmpRpm < 0) tmpRpm = 0;
    //         } else if(tmpRpm < -1000){
    //             tmpRpm+=100;
    //         } else if(tmpRpm <= -100){
    //             tmpRpm+=50;
    //         }else if(tmpRpm < 0){
    //             tmpRpm+=10;
    //             if(tmpRpm > 0) tmpRpm = 0;
    //         }
    //         setMotVal(F_R, tmpRpm);
    //     }
    //     if(gf_MtReqU[2].req){
    //         tmpRpm = getCurMotVal(R_L);
    //         if(tmpRpm == 0){
    //             gf_MtReqU[1].req = false;
    //         }else if(tmpRpm > 1000){
    //             tmpRpm-=100;
    //         } else if(tmpRpm >= 100){
    //             tmpRpm-=50;
    //         } else if(tmpRpm > 0){
    //             tmpRpm-=10;
    //             if(tmpRpm < 0) tmpRpm = 0;
    //         } else if(tmpRpm < -1000){
    //             tmpRpm+=100;
    //         } else if(tmpRpm <= -100){
    //             tmpRpm+=50;
    //         }else if(tmpRpm < 0){
    //             tmpRpm+=10;
    //             if(tmpRpm > 0) tmpRpm = 0;
    //         }
    //         setMotVal(R_L, tmpRpm);
    //     }
    // }
    }

    if(chkSWUserOpeRE(HbUserOpe::FLT_ON)){
        if(stat == IDLE){
            gf_AxReq[0].bf.req = true;
            gf_AxReq[1].bf.req = true;
            gf_AxReq[0].bf.val = getHvAxl(FRONT) / 2;
            gf_AxReq[1].bf.val = getHvAxl(REAR) / 2;
            setStateF(UPPER_IDLE);
        }
        else if(stat == UPPER_IDLE){
            gf_AxReq[0].bf.req = true;
            gf_AxReq[1].bf.req = true;
            gf_AxReq[0].bf.val = getHvAxl(FRONT);
            gf_AxReq[1].bf.val = getHvAxl(REAR);
            setStateF(TAKE_OFF);
        }
        sp.printf("FLT_ON Push \r\n");
    }
    if(chkSWUserOpeRE(HbUserOpe::FLT_OFF)){
        if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE || stat == UPPER_IDLE || stat == IDLE){
            for(int i = 0; i < 2; ++i){
                if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
                    setHvAxl((eEgPos)i, gf_AxReq[i].bf.val);
                }
                gf_AxReq[i].bf.req = true;
                gf_AxReq[i].bf.val = 0;
            }
            if(stat == DRIVE){
                setStateF(EMGGND);
            } else {
                setStateF(GROUND);
            }
        }
        sp.printf("FLT_OFF Push \r\n");
    }
    //if(chkSWUserOpe(HbUserOpe::F_ENG_UP)){
    if(chkSWUserOpeRE(HbUserOpe::F_ENG_UP)){
        if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
            gf_BlinkLED = true;
            INT16 step = en_srv_adj_step[0];
            tmpAxl = getAccelVal(FRONT);
            tmpAxl+=step;
            if(tmpAxl > MAX_VAL_12BIT){
                tmpAxl = MAX_VAL_12BIT;
            }
            gf_AxReq[0].bf.req = true;
            gf_AxReq[0].bf.val = tmpAxl;
        }
    }
    //if(chkSWUserOpe(HbUserOpe::F_ENG_DOWN)){
    if(chkSWUserOpeRE(HbUserOpe::F_ENG_DOWN)){
        if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
            gf_BlinkLED = true;
            INT16 step = en_srv_adj_step[0];
            tmpAxl = getAccelVal(FRONT);
            tmpAxl-=step;
            if(tmpAxl < 0){
                tmpAxl = 0;
            }
            gf_AxReq[0].bf.req = true;
            gf_AxReq[0].bf.val = tmpAxl;
        }
    }
    //if(chkSWUserOpe(HbUserOpe::R_ENG_UP)){
    if(chkSWUserOpeRE(HbUserOpe::R_ENG_UP)){
        if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
            INT16 step = en_srv_adj_step[1];
            gf_BlinkLED = true;
            tmpAxl = getAccelVal(REAR);
            tmpAxl+=step;
            if(tmpAxl > MAX_VAL_12BIT){
                tmpAxl = MAX_VAL_12BIT;
            }
            gf_AxReq[1].bf.req = true;
            gf_AxReq[1].bf.val = tmpAxl;
        }
    }
    //if(chkSWUserOpe(HbUserOpe::R_ENG_DOWN)){
    if(chkSWUserOpeRE(HbUserOpe::R_ENG_DOWN)){
        if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
            INT16 step = en_srv_adj_step[1];
            gf_BlinkLED = true;
            tmpAxl = getAccelVal(REAR);
            tmpAxl-=step;
            if(tmpAxl < 0){
                tmpAxl = 0;
            }
            gf_AxReq[1].bf.req = true;
            gf_AxReq[1].bf.val = tmpAxl;
        }
    }
    if(chkSWUserOpeRE(HbUserOpe::CTRL_SW)){
        bool flg = false;
        switch (g_PidPara.mode)
        {
            case PID_0_OFF:
                g_PidPara.mode = PID_0_ON;
                att->resetValue();
                calAtt();
                flg = true;
                break;
            case PID_1_OFF:
                g_PidPara.mode = PID_1_ON;
                flg = true;
                break;
            case PID_0_ON:
                /* code */
                g_PidPara.mode = PID_0_OFF;
                break;
            case PID_1_ON:
                g_PidPara.mode = PID_1_OFF;
                break;
            default:
                g_PidPara.mode = PID_0_OFF;
                break;
        }
        sp.printf("CTRL_SW Push : %d \r\n", flg);
    }
    // if(chkSWUserOpe(HbUserOpe::ALL_STOP)){
    //     // モーター
    //     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);
    // }        
}
// ユーザー操作関連end
//------------------------------------------------------

//======================================================
//コンストラクタ
//======================================================
HbManager::HbManager(){
    //メンバクラスインスタンス
    eng[0]  = new HbEngine(0);
    eng[1]  = new HbEngine(1);
    eng[0]->setHoverAccel(1500);
    eng[1]->setHoverAccel(1500);
    att     = new HbAttitude(2.0 , 2.0 , 0 , 0);//パラメータ outP P I D

    accelVal[0] = 0;
    accelVal[1] = 0;
    for(int i = 0; i < 4; ++i){
        motorOfsVal[i] = 0;
    }
//    mot[0]  = new HbMotor(0);
//    mot[1]  = new HbMotor(1);
//    mot[2]  = new HbMotor(2);
//    mot[3]  = new HbMotor(3);

    // subProp[0] = new HbSubProp(F_L);
    // subProp[0]->setCoef(IN, 0.00004,    0.0401, 29.262);
    // subProp[0]->setCoef(OUT,0.00004,    0.1336, -69.184);

    // subProp[1] = new HbSubProp(F_R);
    // subProp[1]->setCoef(IN, 0.00004,    0.0151, 70.947);
    // subProp[1]->setCoef(OUT,0.00004,    0.1336, -69.184);
    
    // subProp[2] = new HbSubProp(R_L);
    // subProp[2]->setCoef(IN, 0.00004,    0.0244, 53.954);
    // subProp[2]->setCoef(OUT,0.00003,    0.1579, -114.22);
    
    // subProp[3] = new HbSubProp(R_R);
    // subProp[3]->setCoef(IN, 0.00004,    0.0111, 82.044);
    // subProp[3]->setCoef(OUT,0.00003,    0.1617, -123.27);

    subProp[0] = new HbSubProp(F_L);
    subProp[0]->setCoef(IN, 0.00004,    0.0786,     -5.248);
    subProp[0]->setCoef(OUT,0.00005,    0.0112,     104.45);

    subProp[1] = new HbSubProp(F_R);
    subProp[1]->setCoef(IN, 0.00005,    0.0095,     126.11);
    subProp[1]->setCoef(OUT,0.00004,    0.0798,     -2.1167);
    
    subProp[2] = new HbSubProp(R_L);
    subProp[2]->setCoef(IN, 0.00004,    0.1039,     -50.946);
    subProp[2]->setCoef(OUT,0.00004,    0.0886,     -21.033);
    
    subProp[3] = new HbSubProp(R_R);
    subProp[3]->setCoef(IN, 0.00005,    0.0108,     105.15);
    subProp[3]->setCoef(OUT,0.00004,    0.0456,     50.611);

    imu     = new Imu(p28,p27);
    nowTrgtAng = 0.0f;
    ope = new HbUserOpe();

    for(int i = 0; i < 2; ++i){
        en_srv_step[i] = 30;
        en_srv_adj_step[i] = 20;
    }
    mot_brk = DEF_BRK_RPM;
    ang_rate_brk = DEF_BRK_ANG / static_cast<float>(UPDATE_RATE);
    // //初期化    
    // motorVal[0]=0;
    // motorVal[1]=0;
    // motorVal[2]=0;
    // motorVal[3]=0;
}