File content as of revision 141:725321fe2949:

#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_F / 180.0f);
        agoffset[4] +=(lsm.gy * M_PI_F / 180.0f); 
        agoffset[5] +=(lsm.gz * M_PI_F / 180.0f); 
        palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
        magref(0) += lsm.mx;
        magref(1) += lsm.my;
        magref(2) += lsm.mz;
    }
    for(int i = 0;i<6;i++){
        agoffset[i] /= float(n_init);
    }
    magref /= 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);
    
    Matrix3f Rgpspos;
    setDiag(Rgpspos,1.0f);
    
    Matrix3f Rgpsvel;
    Rgpsvel(0,0) = 0.01f;
    Rgpsvel(1,1) = 0.01f;
    Rgpsvel(2,2) = 0.1f;
    
    MatrixXf Rgps(5,5);
    setDiag(Rgps,0.05f);
    Rgps(3,3) = 0.1f;
    Rgps(4,4) = 0.1f;
    
    float dynAccCriteria = 0.4f;
    Matrix3f Racc;
    setDiag(Racc,100.0f);
    Matrix3f RaccDyn;
    setDiag(RaccDyn,5000.0f);

    Matrix<float, 1, 1> Rheading;
    Rheading(0, 0) = 0.01f;
    
    _t.start();
    //センサ正常性チェック
    if(hilFlag == false){
        while(1){
            float tstart = _t.read();
            getIMUval();
            getGPSval();
            eskf.updateNominal(acc, gyro, att_dt);
            eskf.updateErrState(acc, gyro, att_dt);
            eskf.updateGPS(pi, palt, vi, Rgps);
            float heading = std::atan2(-mag(2), mag(0));
            eskf.updateHeading(heading, Rheading);
            Matrix3f Raccpreflight;
            setDiag(Raccpreflight, 5.0f);
            eskf.updateAcc(acc, Raccpreflight);
            
            bool preflightCheck = true;
            Vector3f gyroBias = eskf.getGyroBias();
            if(fabsf(gyro(0)-gyroBias(0))>2.0f || fabsf(gyro(1)-gyroBias(1))>2.0f || fabsf(gyro(2)-gyroBias(2))>2.0f){
                preflightCheck = false;
                twelite.serial.printf("PreFlight Check : high gyro value\r\n");
            }
            Vector3f accBias = eskf.getAccBias();
            if(fabsf(acc(0)-accBias(0))>2.0f || fabsf(acc(1)-gyroBias(1))>2.0f){
                preflightCheck = false;
                twelite.serial.printf("PreFlight Check : high acc value\r\n");
            }
            Vector3f vihat = eskf.getVihat();
            if(fabsf(vihat(0))>2.0f || fabsf(vihat(1))>2.0f||fabsf(vihat(2))>2.0f){
                preflightCheck = false;
                twelite.serial.printf("PreFlight Check : high velocity value\r\n");
            }
            Vector3f pihat = eskf.getPihat();
            if(fabsf(pihat(0))>2.0f || fabsf(pihat(1))>2.0f||fabsf(pihat(2))>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(acc, gyro, att_dt);
        eskf.updateErrState(acc, 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){
            Vector3f euler = eskf.computeAngles();
            if(fabsf(euler(1))<30.0f*M_PI_F/180.0f){
                headingUpdateFlag = true;
                theading = _t.read();
            }
        }
        
        if(gpsUpdateFlag == true){
            eskf.updateGPS(pi, palt, vi, Rgps);
            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
            //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
        }else if(headingUpdateFlag == true){
                float heading = std::atan2(-mag(1),mag(0));
                eskf.updateHeading(heading,Rheading);
        }else{
            Vector3f dynacc = eskf.calcDynAcc(acc);
            dynaccnorm2 = dynacc.squaredNorm();
            //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2));
            if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
                eskf.updateAcc(acc, RaccDyn);
            }else{
                eskf.updateAcc(acc, Racc);
            }
        }
        PIDtick.loop(); 
        
        //制御時間を計測
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}