File content as of revision 139:b378528c05f2:

#include "global.hpp"

void run()
{
//    wait(0.5);
//
//    //センサの初期化・ジャイロバイアス・加速度スケールの取得
//    int n_init = 1000;
//    for(int i = 0;i<n_init;i++){
//        lsm.readAccel();
//        lsm.readMag();
//        lsm.readGyro();
//        agoffset[0] += lsm.ax * 9.8f;
//        agoffset[1] += lsm.ay * 9.8f;
//        agoffset[2] += lsm.az * 9.8f-9.8f;
//        agoffset[3] +=(lsm.gx * M_PI / 180.0f);
//        agoffset[4] +=(lsm.gy * M_PI / 180.0f); 
//        agoffset[5] +=(lsm.gz * M_PI / 180.0f); 
//        palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
//        magref.x += lsm.mx;
//        magref.y += lsm.my;
//        magref.z += lsm.mz;
//    }
//    for(int i = 0;i<6;i++){
//        agoffset[i] /= float(n_init);
//    }
//    magref.x /= float(n_init);
//    magref.y /= float(n_init);
//    magref.z /= float(n_init);
//    palt0 /= float(n_init);
//    twelite.serial.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
//    
//    //ESKFの共分散設定
//    eskf.setGravity(0.0f,0.0f,9.8f);
//    eskf.setPhatPosition(0.1f);
//    eskf.setPhatVelocity(0.1f);
//    eskf.setPhatAngleError(0.5f);
//    eskf.setPhatAccBias(0.001f);
//    eskf.setPhatGyroBias(0.001f);
//    eskf.setPhatGravity(0.0000001f);
//    
//    eskf.setQVelocity(0.001f);
//    eskf.setQAngleError(0.0000025f);
//    eskf.setQAccBias(0.000001f);
//    eskf.setQGyroBias(0.000001f);
//    
//    Matrix Rgpspos(3,3);
//    setDiag(Rgpspos,1.0f);
//    
//    Matrix Rgpsvel(3,3);
//    Rgpsvel(1,1) = 0.01f;
//    Rgpsvel(2,2) = 0.01f;
//    Rgpsvel(3,3) = 0.1f;
//    
//    Matrix Rgps(5,5);
//    setDiag(Rgps,0.05f);
//    Rgps(4,4) = 0.1f;
//    Rgps(5,5) = 0.1f;
//    
//    float dynAccCriteria = 0.4f;
//    Matrix Racc(3,3);
//    setDiag(Racc,100.0f);
//    Matrix RaccDyn(3,3);
//    setDiag(RaccDyn,5000.0f);
//
//    Matrix Rheading(1,1);
//    Rheading(1,1) = 0.01f;
//    
//    _t.start();
//    //センサ正常性チェック
//    if(hilFlag == false){
//        while(1){
//            float tstart = _t.read();
//            getIMUval();
//            getGPSval();
//            eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
//            eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
//            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
//            float heading = atan2f(-mag.y,mag.x);
//            eskf.updateHeading(heading,Rheading);
//            Matrix Raccpreflight(3,3);
//            setDiag(Raccpreflight,5.0f);
//            eskf.updateAcc(MatrixMath::Vector2mat(acc),Raccpreflight);
//            
//            bool preflightCheck = true;
//            Matrix gyroBias = eskf.getGyroBias();
//            if(fabsf(gyro.x-gyroBias(1,1))>2.0f || fabsf(gyro.y-gyroBias(2,1))>2.0f || fabsf(gyro.z-gyroBias(3,1))>2.0f){
//                preflightCheck = false;
//                twelite.serial.printf("PreFlight Check : high gyro value\r\n");
//            }
//            Matrix accBias = eskf.getAccBias();
//            if(fabsf(acc.x-accBias(1,1))>2.0f || fabsf(acc.y-gyroBias(2,1))>2.0f){
//                preflightCheck = false;
//                twelite.serial.printf("PreFlight Check : high acc value\r\n");
//            }
//            Matrix vihat = eskf.getVihat();
//            if(fabsf(vihat(1,1))>2.0f || fabsf(vihat(2,1))>2.0f||fabsf(vihat(3,1))>2.0f){
//                preflightCheck = false;
//                twelite.serial.printf("PreFlight Check : high velocity value\r\n");
//            }
//            Matrix pihat = eskf.getPihat();
//            if(fabsf(pihat(1,1))>2.0f || fabsf(pihat(2,1))>2.0f||fabsf(pihat(3,1))>2.0f){
//                preflightCheck = false;
//                twelite.serial.printf("PreFlight Check : not home position\r\n");
//            }
//            if(sbus.failSafe){
//                preflightCheck = false;
//                twelite.serial.printf("PreFlight Check : no RC\r\n");
//            }
//            // sbusデータの読み込み
//            for (int i =0 ; i < 16;i ++){
//                rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
//            }
//            if (rc[4]>-0.3f && rc[6] < -0.3f){
//                preflightCheck = false;
//                twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n");
//            }
//            if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){
//                preflightCheck = false;
//                twelite.serial.printf("PreFlight Check : no gps lock\r\n");
//            }
//            
//            if(preflightCheck == true){
//                break;
//            }
//        }
//    }
//    twelite.serial.printf("PreFlight Check Completed\r\n");
//    //usaPack通信開始
//    pc.Subscribe(0000, &(vp));
//
//    //制御ループのアタッチ
//    LoopTicker PIDtick;
//    PIDtick.attach(calcServoOut,PID_dt);
//    
//    float tgps = _t.read();
//    float theading = _t.read();
//    while(1)
//    {
//        float tstart = _t.read();
//        //センサの値を取得
//        if(hilFlag == true){
//            getHilIMUval();
//        }else{
//            getIMUval();
//        }
//        //ekfの更新
//        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
//        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
//
//        if(hilFlag == true){
//            if(tstart-tgps>0.2f){
//                getHilGPSval();
//                tgps = _t.read();
//                gpsUpdateFlag = true;
//            }else{
//                gpsUpdateFlag = false;
//            }
//        }else{
//            if(userButton.read()==1)
//            {
//                gpsLlh0Fixed = false;
//            };  
//            getGPSval();
//        }
//        
//        headingUpdateFlag = false;
//        if(tstart-theading>0.05f){
//            Matrix euler = eskf.computeAngles();
//            if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
//                headingUpdateFlag = true;
//                theading = _t.read();
//            }
//        }
//        
//        if(gpsUpdateFlag == true){
//            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
//            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
//            //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
//        }else if(headingUpdateFlag == true){
//                float heading = atan2f(-mag.y,mag.x);
//                eskf.updateHeading(heading,Rheading);
//        }else{
//            Matrix dynacc = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
//            dynaccnorm2 = dynacc(1,1)*dynacc(1,1)+dynacc(2,1)*dynacc(2,1)+dynacc(3,1)*dynacc(3,1);
//            //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2));
//            if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
//                eskf.updateAcc(MatrixMath::Vector2mat(acc),RaccDyn);
//            }else{
//                eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
//            }
//        }
//        PIDtick.loop(); 
//        
//        //制御時間を計測
//        float tend = _t.read();
//        att_dt = (tend-tstart);
//    }
}