File content as of revision 55:e245227d9fea:

#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;
        }
    }
}
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, ",pidRtn, nowAngle, nowRate);
            sp.printf("PID FB:%f, %f, %f,",pidRtn, nowAngle, nowRate);
            //sp.printf("%f,",nowAngle);
            //sp.printf("%f,",nowRate);
        }
    }
    
    //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
    // 現在、望む角速度から正確なプロペラ回転数に変換する方法を持たないため、得られた値から決まった係数で回転数に変換
    // // float fl = pidRtn * -1;
    // // float fr = pidRtn;
    // // float bl = pidRtn;
    // // float br = pidRtn * -1;
    // // 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;

    // // 推力は回転数の二乗に比例するので、得られた望む角速度の平方根に係数かけて回転数とする
    // float tmpRtn = abs(pidRtn);
    // tmpRtn = sqrtf(tmpRtn);
    // if(pidRtn < 0){
    //     tmpRtn *= -1;
    // }
    // pidRtn = tmpRtn;
    float fl, fr, bl, br;
    //float fl = pidRtn * 2.25 * -1;
    //float fr = pidRtn * 2.25;
    //float bl = pidRtn * 2.25;
    //float br = pidRtn * 2.25 * -1;
    // float fl = pidRtn;
    // float fr = -pidRtn;
    // float bl = -pidRtn;
    // float br = pidRtn;
    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;
    }
    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;
    }
    // if(gf_Print.d1.bf.fb){
    //     sp.printf("REQ RPM FL:%d, FR:%d, RL:%d, RR:%d",gf_MtReq[0].val,gf_MtReq[1].val,gf_MtReq[2].val,gf_MtReq[3].val);
    // }
}

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;
    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(dif > 0){
                //    --tmp;
                //}
                //else{
                //    ++tmp;
                //}
                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){
        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)); }

    //DEBUG用FPGAに直接指示
    INT16 pos;
    INT16 typ;
    if(gf_State == SLEEP){
        for(int i = 0; i < 8; ++i){
            pos = i / 2;
            typ = i % 2;
            if(gf_MtReqDct[i].req){
                tmp = subProp[pos]->getValueFpgaMot((eMotType)typ);
                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);
                }
            }
        }
    }
}

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::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,tmpAxl;
        // Front Left 
    if(chkSWUserOpeRE(HbUserOpe::BRK_L)){
        gf_MtReqU[0].req=true;
        tmpRpm = getCurMotVal(F_L);
        if(tmpRpm < BRK_RPM){
            setMotVal(F_L, BRK_RPM);
        }
        //nowTrgtAng -= 1;

    } else if(chkSWUserOpe(HbUserOpe::BRK_L)){
        if(gf_MtReqU[0].req==true){
            setMotVal(F_L, BRK_RPM);
        }
        //nowTrgtAng -= 0.1;
    } 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(chkSWUserOpeRE(HbUserOpe::BRK_R)){
        gf_MtReqU[1].req=true;
        tmpRpm = getCurMotVal(F_R);
        if(tmpRpm < BRK_RPM){
            setMotVal(F_R, BRK_RPM);
        }
        //nowTrgtAng += 1;
    } else if(chkSWUserOpe(HbUserOpe::BRK_R)){
        if(gf_MtReqU[1].req==true){
            setMotVal(F_R, BRK_RPM);
        }
        //nowTrgtAng += 0.1;
    } 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(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(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;
    }
    // //初期化    
    // motorVal[0]=0;
    // motorVal[1]=0;
    // motorVal[2]=0;
    // motorVal[3]=0;
}